TELKOM
NIKA
, Vol.11, No
.3, March 2
0
1
3
, pp. 1436 ~ 1448
ISSN: 2302-4
046
1436
Re
cei
v
ed O
c
t
ober 1
8
, 201
2; Revi
se
d Ja
nuary 17, 20
1
3
; Acce
pted Janua
ry 2
8
, 20
13
Kalman Filter Localization Algorithm Based on SDS-
TWR Ra
nging
Liu Wei*, Zou Jian, Wan
g
Chun
zhi*
1
, Xu Hui
Schoo
l of Com
puter Scie
nce,
Hub
e
i Un
iversit
y
of T
e
chnolo
g
y
, W
uha
n, Hub
e
i 43
00
68, Chi
n
a
*Corres
p
o
ndi
n
g
author, e-ma
i
l
: liu
w
e
i0
01@v
i
p.com
A
b
st
r
a
ct
Node
loc
a
li
z
a
t
i
on
is the
basi
c
mec
h
a
n
is
m f
o
r w
i
reless s
e
nsor n
e
tw
orks. W
h
ich l
o
catio
n
data
i
s
indis
p
e
n
sa
ble i
n
formatio
n
in t
he
mon
i
torin
g
of events.
In this paper, w
e
pr
opos
ed a n
e
w
alg
o
rtith
m
w
h
ich is
the Kal
m
a
n
filter local
i
z
a
ti
on
alg
o
rith
m bas
e
d
on SD
S-T
W
R rangi
ng
met
hod i
n
order to
solve the pro
b
l
e
m
of nod
e loc
a
li
zation. F
i
rstly, w
e
use SDS-T
W
R algor
it
h
m
to ran
g
dista
n
c
e
betw
een th
e
archor
nod
es
a
n
d
unkn
o
w
n
nod
e
.
T
hen, w
e
sketchy calcu
l
at
e the
unk
now
n nod
e coor
di
nates usi
ng w
e
ig
hted
maxi
m
u
m
likel
ih
ood
esti
mati
on (W
ML
E) meth
od. L
a
stly, w
e
us
e the Kal
m
a
n
filter alg
o
rith
m to optimi
z
e
the
coord
i
nates
of
the seco
nd ste
p
. T
he
exp
e
ri
mental r
e
sults s
how
that t
he a
l
gorith
m
c
an eff
e
ctively
improv
e
the positioning
accuracy of the system
and ac
hiev
e the des
ir
ed objectiv
es.
Ke
y
w
ords
: SD
S-TWR,
WMLE, Kalman filter
Copy
right
©
2013 Un
ive
r
sita
s Ah
mad
Dah
l
an
. All rig
h
t
s r
ese
rved
.
1. Introduc
tion
Nod
e
po
sitio
n
ing in wi
rel
e
ss se
nsor
net
wo
rk, relys on the di
stribution of a
limited
numbe
r of anch
o
r no
de
s [1]. Throug
h some me
chani
sm to determin
e
the unkno
wn n
ode
locatio
n
information in
wi
rele
ss
se
nso
r
netwo
rks. The sen
s
o
r
node
s a
r
e u
s
ually rand
o
m
ly
deploye
d
thro
ugh self-o
rga
n
izatio
n to co
ordin
a
te the
work. The n
o
de localizatio
n pro
b
lem is t
h
e
most ba
sic a
nd most imp
o
rtant pa
rt in the wirel
e
ss
sen
s
o
r
, beca
u
se it is not
perceived
without
the sen
s
o
r
no
des’l
ocation.
The nod
e locali
zation pro
b
lem in sen
s
or networks i
n
clu
d
e
s
rang
e-ba
se
d loca
lization
and ra
nge
-fre
e locali
zation
[2]. The former relie
s on range (or di
stance) mea
s
u
r
ements b
e
tween
node
s that ca
n be estimate
d by the received signal
strength (RSS
) method or th
e time-of-a
rri
val
(TOA) meth
od [3], whil
e the latter use
s
o
n
ly
con
n
e
c
tivity informatio
n.Corre
s
p
ondin
g
ly,
locali
zation al
gorithm
s can
be grou
ped i
n
to rang
e-ba
sed lo
cali
zati
on algo
rithm
s
and ran
ge-f
r
ee
locali
za
-tion algorith
m
s. T
he ran
ge-ba
sed locali
zatio
n
algorithm
s can p
r
ovide h
i
gher lo
cali
za
tion
accuracy tha
n
the range-f
r
ee local
-
ization algor
ithm
s but the
latter is cheape
r and simple
r si
nce
they do n
o
t require
spe
c
ia
l hardware
fo
r ra
ngin
g
[4]. In term
s of
comp
utationa
l paradigm, t
he
locali
zation
algorith
m
s can also be
divided into centralize
d
algorithm
s and distrib
u
ted
algorith
m
s. T
he ce
ntrali
ze
d algorith
m
s requi
re
tran
smissio
n
of all rang
e me
asu
r
em
ents
or
con
n
e
c
tivity informatio
n b
e
twee
n nod
e
s
to a fusio
n
cente
r
(e.g.,
a sin
k
nod
e) for pro
c
e
s
si
ng
,
resulting in large
commu
nicatio
n
energy and
band
width co
n-su
mption and there
b
y sho
r
tenin
g
the lifetime of the whole ne
twork.
The distribute
d
algo
rithms a
r
e en
ergy-efficient and scala
b
le to
the si
ze of th
e networks,
whe
r
e the
wh
ole task
of n
ode lo
cali
zati
on is
coo
p
e
r
atively carrie
d out
by all node
s with local informatio
n exchang
e
betwe
en neig
hbo
ri
ng nod
es. Hence, distri
bu
ted
locali
zation
a
l
gorithm
s a
r
e
much mo
re
attractive for
large
-
scale
sensor
netwo
rks [5].
We
can
dire
ctly see the wirele
ss
sensor no
de lo
cali
zation
mo
del from figure 1. We pro
p
o
se a lo
cali
za
tion
algorith
m
whi
c
h can be divi
ded into thre
e
parts, a
s
followin
g
:
1)
Firstly, we u
s
e SDS-T
W
Ra
lgorithm [6] [7
] base
d
on
CSS (chi
rp spread
spe
c
trum
) tech
nolo
g
y
to range the
distan
ce
s bet
wee
n
ea
ch a
n
ch
or no
de a
nd un
kno
w
n
node.
2)
Secon
d
ly, according to
the di
stan
ce
values
obtai
ned in
step
one, we p
r
o
pose WMLE
algorith
m
ba
sed on M
L
E a
l
gorithm [8] [9] t
hat can roughly calcul
at
ed the un
known nod
e'
s
loc
a
tion information.
3)
Lastly, we ta
ke the estim
a
ted location in
formation
as t
he ob
se
rved
values,
con
s
i
derin
g ki
nd
s
of interfere
n
ce noise pre
s
e
n
t in the environm
ent, we
have esta
blished that the Kalman filter
Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM
NIKA
ISSN:
2302-4
046
Kalm
an Filter Locali
z
atio
n Algorithm
Ba
sed o
n
SDS-TWR Ra
ngin
g
(Liu
Wei)
1437
[10] [11] mod
e
l to furthe
r i
m
prove th
e a
c
cura
cy
of th
e nod
e’s
po
si
tioning, gettin
g
the optim
al
locatio
n
information of the unkno
wn no
d
e
.
2. Ranging b
y
SDS-TWR
Algorithm
w
i
th CSS Tech
nolog
y
SDS-T
W
R,i
s
sho
r
t for
Symmetrical
Dou
b
le-Si
d
ed T
w
o
Way Ran
g
ing.
SDS-T
W
R
algorith
m
de
velops
with
the TWR algorit
h
m
.They are
ba
ed on
clo
c
k syn
c
h
r
oni
zation
mech
ani
sm. Figure 2, 3 shows the working p
r
o
c
e
d
u
r
e of the SDS-TWR algo
ri
thm.Table
s
a
n
d
Figures a
r
e p
r
esented
cent
er, as
sho
w
n
belo
w
and
cited in the man
u
script.
Figure 1. Wireless sen
s
or
node lo
cali
zat
i
on model
SDS-T
W
R al
gorithm
works as follo
win
g
:
Firstly, the archo
r
nod
e A sen
d
s the ran
g
i
ng data to the un-kn
own node B, node
A starts
a timer and begin
s
timing,
whe
n
node B recives the
ranging data
from node A, node B sends
ackno
w
le
dgm
ent frame to
node A.Thi
s
time, records
node B’
s re
spon
se time is
replyB
T
. When
node A reciv
e
s the ACK (ackno
wle
dgm
ent frame)
which send
s b
y
the node of B, node A stops
clo
ck, and re
cords the time of node A a
s
re
plyA
T
. Then, the node B sen
d
s
the rangi
ng data to the
node A, node
B starts a timer and begi
ns timing,
wh
en node A r
e
cive
s the ranging data from
node B, nod
e
A send
s acknowl
edgm
ent
frame to nod
e B. This time, record
s no
de B’s re
sp
o
n
se
time is
re
plyB
T
. Wh
en no
de B recive
s the A
C
K (a
ckno
wl
edgme
n
t fra
m
e)
whi
c
h
send
s by the
node
of A, n
ode B
stop
s
clo
ck, a
nd
re
cords the tim
e
of no
de B
as
roundB
t
. We es
tablis
h the
prop
agatio
n delay of the
rangi
ng sig
n
a
l
in the air fo
r
p
t
. According
to the proce
dure of SDS-
TWR, we can
get the following formula:
2
r
ound
A
p
re
plyB
TT
T
(1)
2
r
ound
B
p
rep
l
y
A
TT
T
(2)
Acco
rdi
ng to the formula (1
), (2), we can
rapi
dly de
rive
the value of
as followi
ng formul
a:
1
*(
)
4
p
r
ound
A
r
e
p
lyB
r
ound
B
r
e
p
lyA
TT
T
T
T
(3)
Evaluation Warning : The document was created with Spire.PDF for Python.
ISSN: 23
02-4
046
TELKOM
NIKA
Vol. 11, No
. 3, March 20
13 : 1436 – 1
448
1438
Figure 2. working p
r
o
c
ed
ure of SDS-
TWR
Figure 3. flowcha
r
t of SDS-TWR algo
rith
m
Und
e
r n
o
rm
a
l
circum
stan
ces, we believ
e
t
hat
the
ra
nging sign
al prop
agatio
n spe
ed
i
s
the same a
s
i
n
the air, accordin
g to the r
angi
ng si
gna
l derived in th
e air propa
ga
tion delay T.
we can obtai
n the distan
ce betwe
en
th
e anchor n
o
d
e
and un
kn
o
w
n no
de
s.
1
**
(
)
*
4
p
r
oun
dA
re
plyB
r
o
undB
re
pl
y
A
dc
T
T
T
T
T
c
(4)
The di
stan
ce
obtained by
the algorith
m
is
only an
ideal.we h
a
ve not take the Clo
c
k
freque
ncy of
the nod
e int
o
co
nsi
deration. As
sume t
hat nod
e A a
nd no
de B'
s
clo
ck f
r
eq
uen
cy
are, stan
ds f
o
r the real p
r
opag
ation de
lay of
the ranging si
gnal
in the air. Finally, we cou
l
d
derive ea
sily as follo
wing f
o
rmul
a:
Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM
NIKA
ISSN:
2302-4
046
Kalm
an Filter Locali
z
atio
n Algorithm
Ba
sed o
n
SDS-TWR Ra
ngin
g
(Liu
Wei)
1439
1
[(
)(
1
)
(
)
(
1
)]
4
p
r
oun
dA
re
pl
y
A
A
r
oun
dB
re
p
l
y
B
B
tt
t
e
t
t
e
(5)
Then, the ra
nging e
rro
r can be exp
r
essed a
s
the differen
c
e betwe
en
p
t
and
p
T
:
pp
p
tt
t
1
[(
)
)
4
ro
undA
re
plyA
A
tt
e
()
]
roun
dB
re
pl
y
B
B
tt
e
.
Substituting
Trou
nd Ai an
d Trou
nd Bi of the above
equation
with formula
(1) and (2
)
respe
c
tively, then we have
the formula (6):
[(
)
(
)
2
(
)
]
4
A
B
r
e
pl
yB
repl
y
A
A
B
p
pp
p
ee
t
t
e
e
t
tt
t
(6)
Here, since the prop
agati
on time between t
he two node
s is very small comp
a
r
ed with
the packet p
r
ocessin
g
time at nodes, we ca
n negl
ect the first term of formula (6).T
hen,
it
become
s
:
()
(
)
/
4
pp
p
A
B
r
e
p
l
y
B
r
e
p
l
y
A
tt
t
e
e
t
t
.
Acorrdin
g to the
p
t
,we ca
n calcul
ate the d
i
stan
ce erro
r with SDS-T
W
R algo
rithm.
()
(
)
**
(
)
*
4
A
B
re
plyB
r
e
ply
A
pp
p
ee
t
t
dc
t
c
t
t
c
(7)
Thro
ugh u
s
in
g SDS-T
W
R
algorith
m
, we
can cal
c
ulat
e distan
ce value
s
betwe
e
n
each
anchor n
ode
and un
kno
w
n
node.Assumi
ng su
ch an e
n
vironm
ent, there a
r
e n an
cho
r
nod
es a
nd
one u
n
kno
w
n
node.
We
ca
n co
nvenie
n
tly get a set of
distan
ce val
ues
with SDS
-
TWR al
gorit
hm.
12
3
{
,
,
...
...
},
[
1
,
]
n
dd
d
d
d
i
d
i
n
.
Whe
n
we cal
c
ulate
d
the distan
ce betw
een the unkn
o
wn no
de an
d anch
o
r nod
es, we
can u
s
e ma
n
y
ways to find the unkn
o
w
n nod
e co
o
r
dinate
s
information, su
ch
as trilaterati
on,
least
squ
a
res method.In thi
s
pa
pe
r, pro
p
o
se
s
weig
hte
d
maximum li
kelih
ood
esti
mation (WM
L
E)
method to cal
c
ulate the
co
ordin
a
tes of the un
kno
w
n
node info
rmat
ion.
3. Weighted
Maxumum Likelihood Estimation
Assu
ming th
a
t
n an
cho
r
n
o
des
dist
ribute
ran
dom
ly
in wirel
e
ss sen
s
or
n
e
two
r
ks. And
the
their co
ordina
tes of the
information a
r
e known : .
11
1
2
2
2
(
,
,
)
,
(
,
,
)
...(
,
,
)
nn
n
x
yz
x
y
z
x
y
z
.
The co
ordi
na
te of
the target node (un
k
no
wn no
de) is
(,
,
)
x
yz
. Having
obtaine
d the
distan
ce valu
es between
each anchor
and target
no
de with SDS-TWR algorith
m
. We can g
e
t
the followin
g
formul
a:
22
2
2
11
1
1
22
2
2
22
2
2
22
2
2
()
(
)
(
)
()
(
)
(
)
.........
()
(
)
(
)
nn
n
n
x
xy
y
z
z
d
x
xy
y
z
z
d
x
xy
y
z
z
d
(8)
Follows the
deform
a
tion
of the formul
a (8):
a
c
q
u
ires the
sum
of both sid
e
s
of the
equatio
n, then averag
e.
22
2
2
11
11
[
(
)(
)(
)
]
nn
ii
i
i
ii
x
xy
y
z
z
d
nn
(9)
Evaluation Warning : The document was created with Spire.PDF for Python.
ISSN: 23
02-4
046
TELKOM
NIKA
Vol. 11, No
. 3, March 20
13 : 1436 – 1
448
1440
Every formula in the formula (8) minu
s equat
ion (9). Afterwa
r
ds,
we can obta
i
n the n
linear e
quatio
ns a
s
formul
a
(10)
sho
w
s.
11
1
11
1
22
2
2
2
2
2
2
11
1
1
1
22
2
11
1
22
2
2
2
2
2
2
22
2
2
1
11
1
()
(
)
(
)
11
[(
)
]
2
11
1
()
(
)
(
)
11
[(
)
]
2
...
..
..
..
..
...
..
nn
n
ii
i
ii
i
n
ii
i
i
i
nn
n
ii
i
ii
i
n
ii
i
i
i
x
xx
y
y
y
z
z
z
nn
n
xy
z
d
x
y
z
d
n
x
xx
y
y
y
z
z
z
nn
n
xy
z
d
x
y
z
d
n
11
1
22
2
2
2
2
2
2
1
.
11
1
()
(
)
(
)
11
[(
)
]
2
nn
n
ni
n
i
n
i
ii
i
n
nn
n
n
ii
i
i
i
x
xx
y
y
y
z
z
z
nn
n
xy
z
d
x
y
z
d
n
(10
)
Formul
a (1
0) is the one
contai
ning th
e ex
pre
s
sion
of n linear
equatio
ns, it can b
e
expre
s
sed as a matrix in the form AX
= B. Of
which, X stands for the target node’
s locati
on
informatio
n. Simultaneo
usly, X is our
g
oal. In th
is
pa
per,
we h
a
ve
unified the
three
-
dim
e
n
s
i
onal
coo
r
din
a
te
system. A, B are availabl
e o
n
matri
xs a
s
formul
a (11),
(12). And
X ca
n be
expre
ssed
as
(,
,
)
T
X
xy
z
.
11
1
11
11
1
1
11
...
...
11
1
1
nn
ii
ii
nn
ni
n
i
ii
n
i
i
n
n
i
i
xx
y
y
nn
A
yy
z
z
nn
zz
n
xx
n
(11
)
22
2
2
2
2
2
2
11
1
1
1
22
2
2
2
2
2
2
22
2
2
1
22
2
2
2
2
2
2
1
1
()
1
()
...
..
..
..
...
..
..
...
..
..
.
1
()
n
ii
i
i
i
n
ii
i
i
i
n
nn
n
n
i
i
i
i
i
xy
z
d
x
y
z
d
n
xy
z
d
x
y
z
d
B
n
xy
z
d
x
y
z
d
n
(12
)
Acco
rdi
ng to the above formula, we hav
e no di
fficulty to calculate the co
ordi
nate
of the
target node. Scilicetly, we
can get
the value of the matrix X as
1
()
TT
X
AA
A
B
.
Thro
ugh ma
ximum likelih
ood estimati
on method,
we can cursory derive the target
node’
s
coo
r
di
nate informati
on. Du
e to errors in th
e ra
nging
pro
c
e
s
s, in this
pap
er, we
propo
se a
weig
hted ma
ximum likelih
ood e
s
timatio
n
. It is a good solution to th
e error a
c
cu
mulated.
In orde
r to re
solve the
pro
b
lem of the e
rro
r a
c
cumul
a
ted, in the p
aper,
w
e p
r
op
ose th
e
weig
hted ma
ximum likelih
ood e
s
timatio
n
method, according to th
e accuracy o
f
the location
o
f
each an
ch
or
node i
n
the m
a
ximum likeli
hood
estimat
e
s
by u
s
in
g d
i
fferent wei
g
h
t
ing facto
r
to the
positio
n cal
c
ulation to im
prove p
o
sitio
n
ing a
c
curacy. Generally
spe
a
ki
ng, we
ighted maxim
u
m
likeliho
od e
s
timates ha
s th
e followin
g
eq
uation:
1
()
TT
X
AW
A
A
W
B
.
Whe
r
e
W is the wei
ghting
matrixand the
inverse m
a
tri
x
of ranging e
rro
r equ
ation
matrix.
Frontly, we h
a
ve analyze
d
the SDS-TWR algo
ri
thm’s rangin
g
erro
r. Gene
rally. We can
see fro
m
the formul
a (7
).
Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM
NIKA
ISSN:
2302-4
046
Kalm
an Filter Locali
z
atio
n Algorithm
Ba
sed o
n
SDS-TWR Ra
ngin
g
(Liu
Wei)
1441
We record th
at is the variance of the ra
nging e
r
ror:
2
11
1
()
1
nn
ii
i
ii
Sd
d
n
(13
)
1
00
0
n
S
W
s
(14
)
With weighte
d
maximum
likeliho
od e
s
t
i
mation meth
od, we exp
e
c
t to improve the
accuracy. Lat
er, throu
gh e
x
perime
n
ts si
mulation,
we
will have a co
ncrete analy
s
is of this idea.
4. Kalman Filter
Rud
o
lf Emil Kalman
who i
s
mathe
m
atician of
United
States propo
sed Kal
m
an f
ilter.The
Kalman filter was extract
ed from the
sign
al
s me
asured th
rou
g
h
the obse
r
va
tion method
to
estimate the
required si
gnal and
ca
n handle o
n
e
-dim
en
siona
l or non-stat
ionary or m
u
lti-
dimen
s
ion
a
l random p
r
o
c
e
ss. The data
stora
ge ca
pa
ci
ty of kalman filter is also small. Becau
s
e
of the advantage
s of Kalman filter, the theory ha
s be
en applie
d in many engin
e
e
ring field
s
after
it was propo
sed. The Kal
m
an filt
er, also known as linear qua
drat
ic estimatio
n
(LQE), is
an
algorith
m
whi
c
h u
s
e
s
a
se
ries of mea
s
urem
ents
ob
served ove
r
ti
me, co
ntainin
g
noi
se (ra
n
d
o
m
variation
s
)
an
d other i
n
a
c
cura
cie
s
, and
prod
uces
es
ti
mates of u
n
known varia
b
l
e
s that ten
d
to be
more p
r
e
c
ise
than those that would be
based
on a
singl
e measu
r
eme
n
t alone
. More formal
ly,
the Kalman filter ope
rate
s recursively o
n
stre
ams
of noisy inp
u
t data to prod
uce a statisti
cal
l
y
optimal estim
a
te of the underlying
syste
m
state. The filter is nam
ed
for Rudolf (Rudy) E. Kálmán,
one of the p
r
imary d
e
vel
opers of its t
heory. Th
e Kalman filter h
a
s nu
merou
s
application
s
in
techn
o
logy. A common
appli
c
ation i
s
for guid
a
n
c
e, navigatio
n and co
ntrol of vehicles,
particularly aircraft and
spacecraft. Furtherm
o
re, the
Kalman filter i
s
a wi
dely applied concept
in
time serie
s
a
nalysi
s
use
d
in fields su
ch
as signal p
r
oce
s
sing an
d
econom
etri
cs. The algorit
h
m
works in a two-ste
p
process: in
the pre
d
i
ction ste
p
, the Kalman f
ilter produ
ce
s
estimate
s of the
curre
n
t state variables, along with their un
ce
rtai
nties. Once
the outcome of the
next
measurement
(ne
c
e
s
sarily
corru
p
ted wi
th some
am
ount of erro
r, includi
ng ra
ndom n
o
ise) is
observed, the
s
e estimate
s are upd
ated usin
g a we
igh
t
ed average, with more wei
ght being given
to estimate
s with high
er certainty. Because of t
he algorithm'
s
recursive n
a
ture, it can run in
rea
l
time usin
g o
n
ly the pre
s
ent input m
easure
m
ent
s and the p
r
eviously calculated state;
no
addition
al pa
st informatio
n
is req
u
ired. From a
the
o
retical sta
ndp
oint, the main assum
p
tion
of
the Kalman filter is that th
e unde
rlying
system i
s
a linear
dynami
c
al system a
n
d that all error
terms and measurement
s
have
a Gau
ssi
an
di
stri
butio
n (of
t
en a multivariate G
a
u
s
sian
distrib
u
tion
).
Extension
s
and gene
rali
za
tions to
the
method have
also been d
e
velope
d, such as
the Extended Kalman Filter and the Un
scented Kalm
an
filter which work on no
nlinea
r syste
m
s.
The u
nde
rlying mo
del i
s
a Bayesi
an
model
simila
r to a hid
den
Markov mo
d
e
l but whe
r
e
the
state sp
ace of the latent
variabl
e
s
is continuo
us an
d whe
r
e all latent and ob
serve
d
varia
b
le
s
have Gau
s
sia
n
distrib
u
tion
s.
Usi
ng SDS
-
T
W
R to
range
is vulnerable
to t
he impact
of the environ
ment, it will produce
white noi
se. We nee
d to eliminate the white noise
by usin
g the Kalman filter, in
orde
r to improve
the positio
nin
g
accuracy of
the system.
In order to simulate the perform
an
ce o
f
the location estimation system, we formulate
the location
estimation a
s
a filtering problem in
stat
e-spa
c
e. The
mathematical model for the
target motion
and the mea
s
urem
ent vect
or on the targ
et are den
ote
d
by following
formula:
11
(,
)
,
(
0
,
)
kk
k
k
k
X
fX
W
W
N
Q
(15
)
Evaluation Warning : The document was created with Spire.PDF for Python.
ISSN: 23
02-4
046
TELKOM
NIKA
Vol. 11, No
. 3, March 20
13 : 1436 – 1
448
1442
(,
)
,
(
0
,
)
kk
k
k
k
y
hx
v
v
N
R
(16
)
Among the two formula, all
of the param
eters’ d
e
scrip
tion sho
w
n in
Table 1 .
Table 1. Para
meters’ De
scription
Parameters
Des
c
r
i
p
ti
on
k
x
State vector
()
f
state transition matrix
k
w
target model nois
e
k
Q
target model nois
e
covariance matrix
k
y
actual measurem
ent equation
()
h
measurement m
a
trix
k
v
measurement no
ise
k
R
measurement no
ise covariance m
a
trix
Let
()
[
,
,
,
,
,
]
x
yz
T
kk
k
k
k
k
x
kx
y
z
v
v
v
,
,,
kk
k
x
yz
,
x
k
v
,
,
yz
kk
vv
,
resp
ectively stan
d for
target no
de i
n
three di
re
cti
ons of the
displace
ment a
n
d
spe
ed e
s
ti
mated value i
n
the co
ordi
n
a
te
sy
st
em in
k moment
.
Firstly, we e
s
tablish the st
ate equation
and di
scretizate according
to the displa
ceme
nt
and velocity of the system above location inform
a
t
ion.Positioni
ng system
st
atus equ
atio
n as
following:
(1
)
(
)
(
)
X
kX
k
W
k
(17
)
()
()
()
Ok
X
k
V
k
(18
)
In formula (17), (18).
()
X
k
is a state vector whi
c
h is the target
node’
s location
informatio
n n
eed
s to be
optimize
d
.
()
[
,
,
,
,
,
]
x
yz
T
kk
k
k
k
k
x
kx
y
z
v
v
v
;
is a sy
st
em
mat
r
ix
;
()
Ok
is a ob
servat
ion vecto
r
wh
ich ob
se
rve
the targ
et nod
e’s lo
cation i
n
formatio
n.
()
Ok
can
be formul
ated
as followi
ng:
()
(
,
,
)
x
yz
T
kk
k
Ok
o
o
o
.
x
k
o
,
y
k
o
,
z
k
o
re
spe
c
tively stands for o
b
se
rved di
splacement values in the coordi
nate
system in t
h
ree
dire
ctio
ns.
is a ma
trix of output.
()
Wk
and
()
Vk
r
e
spec
tively
rep
r
e
s
entativ
e the state no
ise an
d ob
servati
on noise. And satisfy th
e followin
g
eq
uation:
[
(
)]
[
(
)]
0
EW
k
E
V
k
(19
)
[(
)(
)
]
T
EW
k
W
k
Q
(20
)
[(
)(
)
]
T
EV
k
V
k
R
(21
)
Equation
s
(1
9), (2
0), (2
1) aboved
rep
r
ese
n
t that
()
Wk
an
d
()
Vk
are inde
pe
ndently
zero mea
n
white noise se
quen
ce.
The stati
s
tica
l prope
rtie
s of the initial val
ue of the state vecto
r
X (0) is d
e
fined a
s
:
0
[(
0
)
]
EX
u
,
00
0
{[
(0)
]
[
(
0)
]
}
T
EX
u
X
u
p
.
Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM
NIKA
ISSN:
2302-4
046
Kalm
an Filter Locali
z
atio
n Algorithm
Ba
sed o
n
SDS-TWR Ra
ngin
g
(Liu
Wei)
1443
Kalman filter is divided into two main par
ts, na
mely, the
foreca
sting pro
c
e
ss an
d
corre
c
tion p
r
o
c
e
ss. Th
ere a
r
e two mai
n
e
quation
s
a
s
followin
g
in the forecastin
g pro
c
e
ss.
State equatio
n:
(|
1
)
(
1
|
1
)
Xk
k
X
k
k
(22
)
Predi
ction eq
uation:
(|
1
)
(
1
|
1
)
T
p
kk
p
k
k
Q
(23
)
There are th
ree main eq
ua
tions a
s
follo
wing in the
co
rre
ction p
r
o
c
e
ss.
The Kalman
gain eq
uation
:
1
(
)
[
(
|1
)
]
[
(
|1
)
]
TT
Kk
p
k
k
p
k
k
R
(24
)
The optimal e
s
timate of expre
ssi
on on t
he pre
s
e
n
t state:
(|
)
(
1
|
1
)
[
(
)
(
|
1
)
]
k
Xk
k
X
k
k
K
O
k
X
k
k
(25
)
State update
equatio
n:
(|)
[
(
)
]
(
|
1
)
pk
k
E
K
k
Pk
k
(26
)
E stand
s for the unit matrix
.
The followi
ng
formula is th
e three - dim
ensi
onal mo
d
e
l of the obje
c
t movement:
2
1
2
1
1
2
1
1
1
00
2
10
0
0
0
01
0
0
0
00
2
00
1
0
0
00
000
1
0
0
2
01
0
0
1
0
00
00
1
0
0
1
00
00
kk
kk
kk
xx
kk
yy
kk
zz
kk
T
xx
T
T
yy
T
zz
T
T
vv
vv
T
vv
T
T
1
1
1
1
1
1
x
y
z
x
k
y
k
z
k
V
k
V
k
V
k
w
w
w
w
w
w
(27
)
1
0
0000
0
1
0000
00
1
0
00
k
k
x
x
kk
k
yy
kx
k
k
zz
ky
k
k
z
k
x
y
ov
z
ov
v
ov
v
v
(28
)
Initial value selection:
(0
)
0
X
;
Evaluation Warning : The document was created with Spire.PDF for Python.
ISSN: 23
02-4
046
TELKOM
NIKA
Vol. 11, No
. 3, March 20
13 : 1436 – 1
448
1444
1
600000
0
1
60000
00
1
6
000
(0
|
0
)
000
1
600
0000
1
6
0
00000
1
6
P
(29
)
5.
Experime
ntal Re
sults
and An
aly
s
is
In our
experiment, we
u
s
e the
Well
n
ode
Comp
an
y’s WD50
32
N mo
dule
chip with
72MHz hi
gh-spe
ed ARM
and the PAN5375 m
e
a
s
urement mod
u
l
e
. Its commu
nicatio
n
dista
n
ce
can re
ach as far as 800 meters. It uses the
Nan
o
tron uniq
ue chirp sp
re
ad spe
c
tru
m
(CSS)
comm
uni
cati
on technol
og
y, highly integrated
mixe
d
-
sig
nal chip, and works
in the
2.4G
Hz b
and.
The chip provides high
wirel
e
ss com
m
unicati
on
p
e
rform
a
n
c
e,
but also p
r
o
v
ides a
c
cu
ra
te
rangi
ng functi
on. It can be use
d
to develop the r
angin
g
system in wirele
ss sen
s
o
r
netwo
rks wi
th
locatio
n
-a
wa
re function
ality. WD50
32
N module
chip
as sho
w
n in
Figure 4, 5.
Becau
s
e of constraints, we are ha
rdly carr
y out the algorith
m
that use n an
cho
r
nod
e
s
to distribute rand
omly in
sen
s
o
r
netwo
rk. In
a specific lab environment, we use four an
chor
node
s to mo
nitor and p
o
sition the obje
c
t real
-time.
For the di
stri
bution of nod
es, in ord
e
r t
o
simplify the calcul
ation, these n
ode
s pl
ace
d
in
the same heig
h
t in the experime
n
tal scene. T
hen
establi
s
h th
e
three
-
dime
nsi
onal
coo
r
din
a
t
e system, th
e z
coo
r
din
a
tes of e
a
ch n
ode i
s
zero,
so
all node
s are in the three
-
di
mensi
onal
co
ordin
a
te syst
em xoy plane
.
Figure 4. Anchor Node A
Figure 5. WD5032
N Mod
u
l
e
Chip
The expe
rim
ent is divide
d
into three p
a
r
ts.The
first p
a
rt of the exp
e
rime
nt is bu
rning the
SDS-T
W
R al
gorithm into the chip. Th
rough u
s
ing
SDS-T
W
R al
gorithm, we
can me
asure
the
distan
ce
bet
wee
n
the ta
rget nod
e a
n
d
ancho
r no
de
s.The
se
co
nd
part of i
s
u
s
i
ng the
app
ro
ach
of weighte
d
maximum like
lihood e
s
tima
te to obtain
the coo
r
di
nate i
n
formatio
n of the target no
de.
Finally, we ad
opt the Kalman filter algo
rithm to opt
imize the re
sult
s of the coordi
nate inform
ation.
In orde
r to a
c
hieve the pu
rpose of re
al-ti
m
e tr
a
cki
ng a
nd lo
cating, the CSS mo
d
u
le chip send
ba
set of po
sitio
n
ing data to
obtain lo
catio
n
informat
io
n of the target
node inte
rval
0.1 se
con
d
, then
host compute
r
re
cives the
s
e data and p
r
oce
s
s them.
In the experi
m
ent enviro
n
m
ent, we fixed the co
o
r
din
a
tes of the archo
r
nod
es in
clud
ed A,
B, C, D. Then we e
s
tabli
s
h the three
-
di
mensi
onal
co
ordin
a
tes th
a
t
take the ma
in archo
r
nod
e
(arch
o
r
node
A) as t
he o
r
gi
n. A tap is u
s
ed to me
asure the di
stan
ces e
a
ch a
r
ch
or n
ode
s. In that
Evaluation Warning : The document was created with Spire.PDF for Python.
TELKOM
NIKA
ISSN:
2302-4
046
Kalm
an Filter Locali
z
atio
n Algorithm
Ba
sed o
n
SDS-TWR Ra
ngin
g
(Liu
Wei)
1445
ca
se, we ca
n determine
the archo
r
n
ode
s’ (A,B
,C,D) coo
r
dinat
e inform
ation
in the rel
a
tive
coo
r
din
a
te sy
stem by u
s
in
g sp
ace ge
o
m
etry.We
co
nne
ct the ma
in arch
or n
o
d
e
A with the
PC
via the seri
al
line. The sof
t
ware i
n
the PC
ca
n real
-t
ime uploa
d a
nd display the mea
s
urem
ent
results. Ta
bl
es 2 a
nd 3
have re
sp
ect
i
vely giv
en the di
stan
ce
informatio
n a
nd the relati
ve
coo
r
din
a
te sy
stem calculat
ed by
the
coo
r
dinate i
n
form
ation of the ta
pe mea
s
u
r
e f
o
r ea
ch
an
ch
or
node (fo
r
experime
n
tal con
v
enien
ce, the experiment
durin
g all the nodes provid
ed in the same
plane, ta
ke t
he Z coo
r
din
a
tes of all n
ode
s a
s
ze
ro, then the t
h
ree
-
dim
e
n
s
i
onal
coo
r
din
a
te
system
conve
r
ts to a two-di
mensi
onal
co
ordin
a
te syst
em for sim
p
lifying).
Table 2. The
distan
ce
s of each archo
r
n
ode
s
Number
AB
BC
AC BD AD
CD
Distance/m
3.2
2.8
4.65 3.9 4.85
2.1
Table 3. The
coo
r
din
a
te of each archo
r
n
ode
A B
C
D
(0.0000,0.
0000)
(3.2000,0.
0000)
(3.7535,2.
7447)
(2.8988,3.
4563)
Table 4. Experime
n
t results with SDS
-
T
W
R
Number
p
t
/ns
Distance/m
Measureme
p
t
Real distance
Error
(
T)
1
TA 3.4500
11.507672
2.86
0.59
TB 1.8600
6.204141
1.12
0.74
TC
2.0400
6.804532
1.41
0.73
TD
1.5800
5.287818
2.65
1.07
2
TA 2.9800
9.973226
1.86
1.12
TB 2.2400
7.496653
1.58
0.66
TC
2.5600
8.567604
2.04
0.52
TD
3.0800
11.030789
2.54
0.56
3
TA 4.0500
13.554238
3.23
0.82
TB 1.8600
6.224391
1.28
0.58
TC
2.2400
7.496042
1.39
0.85
TD
1.2500
4.183066
1.85
0.60
4
TA 3.2700
10.942951
2.86
0.41
TB 2.2800
7.629908
1.48
0.80
TC
1.2300
4.11613
2.46
1.23
TD
1.8900
6.32478
1.27
0.62
5
TA 2.4600
8.23226
2.04
0.42
TB 1.8700
6.25785
1.23
0.64
TC
2.5600
8.56691
2.98
0.44
TD
2.9800
9.97241
1.89
1.09
6
TA 1.9800
6.62596
1.28
0.70
TB 2.1400
7.16139
2.87
0.73
TC
2.8600
9.57084
3.52
0.66
TD
3.2500
10.87591
4.06
0.81
7
TA 3.2700
10.9429
3.59
0.32
TB 2.5300
8.46651
1.98
0.45
TC
1.6200
5.42124
1.45
0.17
TD
2.8200
9.43698
2.05
0.77
8
TA 1.5800
5.28738
2.34
0.76
TB 2.2400
7.49604
1.98
0.26
TC
3.4200
11.4448
2.88
0.54
TD
4.0500
13.5531
3.15
0.90
9
TA 3.6500
12.2145
2.86
0.79
TB 2.4600
8.23226
2.97
0.51
TC
2.1800
7.29525
3.27
1.09
TD
1.9800
6.62596
2.87
0.89
10
TA 2.9500
9.87202
2.42
0.53
TB 3.2400
10.8425
2.76
0.48
TC
2.6700
8.93501
1.86
0.81
TD
3.0800
10.3071
2.28
0.80
Evaluation Warning : The document was created with Spire.PDF for Python.