Ci a ion: Viana, K.; Zubiza e a, A.;
Diez, M. A Recon igu able
F amewo k o Vehicle Localiza ion
in U ban A eas. Senso s 2022,22, 2595.
h ps://doi.o g/10.3390/s22072595
Academic Edi o : Aboelmagd
Nou eldin
Recei ed: 1 Feb ua y 2022
Accep ed: 25 Ma ch 2022
Published: 28 Ma ch 2022
Publishe ’s No e: MDPI s ays neu al
wi h ega d o ju isdic ional claims in
published maps and ins i u ional a il-
ia ions.
Copy igh : © 2022 by he au ho s.
Licensee MDPI, Basel, Swi ze land.
This a icle is an open access a icle
dis ibu ed unde he e ms and
condi ions o he C ea i e Commons
A ibu ion (CC BY) license (h ps://
c ea i ecommons.o g/licenses/by/
4.0/).
senso s
A icle
A Recon igu able F amewo k o Vehicle Localiza ion in
U ban A eas
Ke man Viana , Asie Zubiza e a * and Mikel Diez
Facul y o Enginee ing in Bilbao, Uni e si y o he Basque Coun y UPV/EHU, 48013 Bilbao, Spain;
[email p o ec ed] (K.V.); [email p o ec ed] (M.D.)
*Co espondence: asie [email p o ec ed]
Abs ac :
Accu a e localiza ion o au onomous ehicle ope a ions is essen ial in dense u ban a eas.
In o de o ensu e sa e y, posi ioning algo i hms should implemen aul de ec ion and allback
s a egies. While many s a egies s op he ehicle once a ailu e is de ec ed, in his wo k a new
amewo k is p oposed ha includes an imp o ed econ igu a ion module o e alua e he ailu e
scena io and o e al e na i e posi ioning s a egies, allowing con inued d i ing in deg aded mode
un il a c i ical ailu e is de ec ed. Fu he mo e, as many ailu es in senso s can be empo a y, such
as GPS signal in e up ion, he p oposed app oach allows he e u n o a non- aul s a e while
ese ing he al e na i e algo i hms used in he empo a y ailu e scena io. The p oposed localiza ion
amewo k is alida ed in a se ies o expe imen s ca ied ou in a simula ion en i onmen . Resul s
demons a e p ope localiza ion o he d i ing ask e en in he p esence o senso ailu e, only
s opping he ehicle when a ully deg aded s a e is achie ed. Mo eo e , econ igu a ion s a egies
ha e p o en o consis en ly ese he accumula ed d i o he al e na i e posi ioning algo i hms,
imp o ing he o e all pe o mance and bounding he mean e o .
Keywo ds: au onomous ehicle; obus localiza ion; econ igu a ion; senso usion
1. In oduc ion
1.1. Mo i a ion
Accu a e localiza ion is a undamen al equi emen o achie e a high le el o au onomy
in na iga ion and posi ioning o any au onomous ehicle [
1
]. To mee his equi emen ,
posi ioning algo i hms ende he aw da a ob ained om he acquisi ion sys em, which is
composed o di e en senso s. This ende ed ou pu is hen used by la e sys ems, such as
he ajec o y planne , o make decisions based on bo h he posi ion o he ehicle and i s
su ounding en i onmen .
In ecen yea s, localiza ion asks ha e ypically been ca ied ou by using global
posi ioning sys ems (GPS) and ine ial na iga ion sys ems (INS), mainly due o hei
a o dabili y and con enience [
2
]. Howe e , GPS/INS posi ioning o au onomous ehicles
equi es ensu ing an app op ia e-quali y GPS signal. This is no always possible in ce ain
a eas, such as dense u ban a eas o zones whe e signals may be in e up ed by all buildings
o unnels, making ehicle localiza ion un eliable.
Fo hese cases, o he localiza ion app oaches ha e been p oposed, mainly di ided
in o wo g oups: (1) local-posi ioning-based app oaches, which use he las known posi ion
o es ima e ela i e localiza ion, and (2) map-aided echniques, which o e global local-
iza ion by using accu a e digi al maps as global e e ences o he ego ehicle along i s
d i ing en i onmen .
One o he ea lies app oaches o local-posi ioning-based app oaches is he use o
dead eckoning echniques, which a e ypically based on ehicle odome y and INS ead-
ings. These echniques ha e been b oadly used in mobile obo localiza ion [
3
] and, e en
hough accu acy is limi ed by wheel slippage, e ain inconsis ency o ehicle-pa ame e
Senso s 2022,22, 2595. h ps://doi.o g/10.3390/s22072595 h ps://www.mdpi.com/jou nal/senso s
Senso s 2022,22, 2595 2 o 16
misma ching, hey a e s ill pa o he localiza ion s a egy o many au onomous ehi-
cles [4,5].
Mo e ecen ly, local posi ioning has also been add essed by analyzing and p ocessing
came a images. This echnique has been named isual odome y (VO) [
6
,
7
]. A simila
app oach, which some au ho s name LiDaR odome y [
8
], can be implemen ed using he
da a cloud p o ided by LiDAR senso s. In hese cases, cloud-poin ma ching o egis a ion
algo i hms, such as he no mal dis ibu ion ans o m (NDT) [
9
], can be used o es ima e
he ela i e o ien a ion and posi ion o he ego ehicle. The use o came as o LiDAR
senso s has he bene i o a oiding d i due o e ain o ehicle pa ame e misma ches
and in ecen yea s ha e gained inc easing in e es due o imp o ed accu acy.
Howe e , he a o emen ioned algo i hms, as hey calcula e ela i e posi ions and
equi e in eg a ion, will always p esen d i when used o es ima e he posi ion o he
ehicle in he global ame. The e o e, hese s a egies on hei own canno gua an ee p ope
long- e m global localiza ion on hei own and need o be combined wi h o he app oaches.
To cope wi h he p e ious limi a ions, map-aided algo i hms ha e been p oposed [
10
].
These echniques ma ch he app oxima e localiza ion o he ego ehicle wi h a p ede ined
mesh o oads, junc ions and elemen s om he d i ing en i onmen . A popula app oach
in his a ea is simul aneous localiza ion and mapping (SLAM), which is ocused on localiza-
ion in unknown scena ios by cons uc ing local maps o he en i onmen while posi ioning
he ehicle in hem [
11
,
12
]. Howe e , in known u ban scena ios, he use o high-de ini ion,
globally e e enced digi al maps can p o ide accu a e global posi ioning [
13
]. Two o he
main app oaches in his ield a e oad ma ching [14] and ea u e na iga ion [15].
Road ma ching algo i hms, based on he p emise ha he ego ehicle’s posi ion will
always be es ic ed o he oad ne wo k, aim o in eg a e p e ious posi ioning da a wi h
spa ial oad ne wo k da a o iden i y he ac ual oad link on which he ehicle is a eling.
Di e en app oaches ha e been made o sol e his issue, and he map-ma ching algo i hms
a e usually di ided in o h ee ca ego ies: geome ic [
16
], opological [
17
] and ad anced
map ma ching [14].
On he o he hand, ea u e na iga ion algo i hms [
18
] consis o a wo-s ep p ocess.
Fi s , hey need o co ec ly iden i y p e iously mapped and loca ed oad elemen s, such
as a ic pos s, ligh s o buildings, su ounding he ego ehicle and cap u ed by he
moun ed came as o LiDAR senso s. O en, his s ep is achie ed by he use o classi ica ions
algo i hms based on a i icial neu al ne wo ks (ANN) [
19
]. Nex , based on he ela i e
dis ance om he ego ehicle o he iden i ied ea u e, he posi ion o he ego ehicle is
globally co ec ed om he p e ious es ima ion based on ela i e calcula ions.
Bo h o hese echniques a e b oadly used in au onomous d i ing global localiza-
ion [
15
,
20
]. Howe e , ea u e na iga ion algo i hms equi e a wo-s ep p ocess along
wi h a e y de ailed digi al map o he d i ing en i onmen , which needs o be upda ed
o be accu a e. Fu he mo e, ea u e na iga ion wo ks on he assump ion ha enough
ea u es exis in he d i ing en i onmen o pe iodically co ec he ela i e localiza ion
o he ehicle, which canno always be gua an eed. On he o he hand, oad-ma ching
echniques a e usually based on simpli ied maps and a e less cos ly in compu a ional e ms.
E en in he case o no ha ing a digi al map o he d i ing en i onmen , mos o he ime
his can be easily buil using geog aphic in o ma ion sys ems (GIS) [
16
]. Thus, i he maps
a e accu a e, he la e app oach can p o ide global posi ioning o he ehicle, making i an
in e es ing complemen o GPS-based posi ioning [
10
]. Mo eo e , he p e iously analyzed
local-posi ioning-based app oaches can be combined wi h map-aided algo i hms o co ec
he accumula ed d i [21–23].
The e o e, local-posi ioning-based app oaches and adi ional GPS/INS app oaches
may be combined using map-aided echniques o ensu e p ope localiza ion in dense u ban
a eas. Fusing di e en senso da a is a widely s udied a ea in he li e a u e [
24
]. In he case
o ehicle localiza ion, he s a egies a e usually di ided in o wo main g oups: op imiza ion
echniques and il e ing me hods. Fo he o me , commonly applied echniques, such as he
Bundle Adjus men [
25
], a e used because o hei consis en and accu a e esul s on a ious
Senso s 2022,22, 2595 3 o 16
scena ios. On he o he hand, il e ing me hods end o be based mainly on ex ended
Kalman il e s (EKF) and i s a ian s hanks o hei con e gence and consis ency [
2
].
Addi ionally, il e s ha e also o e ed excellen esul s by combining di e en senso s
and app oaches [
26
]. Finally, Kalman il e ing also o e s g ea e sa ili y ega ding s a e
es ima ion [2], making i he main choice o da a usion in he localiza ion amewo k.
Howe e , all hese app oaches a e ocused on p o iding accu a e posi ioning assuming
ha he pe cep ion sys em does no ail o p o ides inco ec o low-p ecision da a. When
conside ing au onomous ehicles, he localiza ion sys em no only has o p o ide accu a e
posi ioning bu also be able o cope wi h possible senso mal unc ion o signal loss. In his
case, de ining s a egies ha can consis en ly de ec possible e o s and selec he mos
app op ia e senso da a o pe o m posi ioning is equi ed so ha p ope allback s a egies
can be execu ed i needed. This wo k ocuses on his a ea o esea ch.
1.2. E o De ec ion and Managemen in Vehicle Localiza ion Sys ems
The mos common s a egy o e o de ec ion in he senso inpu s o au onomous
ehicles is based on duplica ion and compa ison app oaches and he use o belie unc ions,
as de ined by Sme s in his T ans e able Belie Model (TMB) [
27
]; i.e., assembling edundan
localiza ion algo i hms aiming o compa e he di e en ou pu s and expose he ailing
senso o algo i hm. Examples o such app oaches can be ound in [
28
,
29
], whe e edundan
in o ma ion is used o de ec co up ed o e oneous da a and ob ain con ol signals ha
a e u he used in a Kalman il e . Sel -assessing Bayesian il e s ha e also been p oposed
o me ge di e en in o ma ion sou ces and imp o e he obus ness, as hey implemen an
e o de ec ion sys em [
30
]. These il e s o en use o he no malized inno a ion squa ed
(NIS) me ic, which akes measu emen in o ma ion o iden i y ou lie s and expose he
ailu e [
31
]. Howe e , hese app oaches a e based on s a is ical alues ha equi e a ce ain
amoun o high-quali y measu emen and numbe o samples o ensu e he co ec de ec ion
o ailu es [
32
]. Mo eo e , because o hei na u e, he use o s a is ical me ics o de ec
measu emen e o s can ha dly ensu e he ins an de ec ion o a ailing module, which may
lead o w ong decision-making. In gene al, mos o he wo k in he ield o e o de ec ion
ends o ocus only on e o -de ec ion consis ency, neglec ing a eas o g ea in e es such as
decision-making s a egies and al e na i e-posi ioning-s a egy accu acy.
Once he e o has been de ec ed, a decision-making algo i hm mus de e mine i s
e ec on he au onomous ehicle. Some au ho s p opose g aph-based app oaches unde
he op imiza ion amewo k [
33
] and ex ended Kalman il e (EKF) [
30
], which can ensu e
localiza ion in he sho e m in he p esence o e o s, e en when such e o s a e no
de ec ed. Howe e , hese s a egies a e no alid once he e o du a ion ex ends o longe
pe iods o ime. The e o e, in he p esence o ailu es o longe du a ion, mos o he wo k
in his a ea p oposes o immedia ely s op he ehicles in a sa e place in case o any ailu e
o deg ada ion [34,35].
Howe e , ailu es in localiza ion modules, pa icula ly in dense u ban a eas, can be
o empo al na u e due o signal deg ada ion. This can be due o senso occlusion, such
as he case o came as o GPS signals, and, in mos cases, he signal deg ada ion occu s
in pa icula a eas. Hence, immedia ely s opping he ehicle in hese scena ios limi s he
po en ial o au onomous ehicles, and some esea ch has p oposed implemen ing allback
s a egies based on econ igu ing he posi ioning algo i hms so ha he ehicle can con inue
ope a ing in a deg aded mode o a p ede ined sho pe iod o ime [
36
] un il ei he he
occlusion o deg aded signal is esol ed o he e o is de ec ed as c i ical and a s op has o
be execu ed.
In his a ea, wo ks ha p opose al e na i es a e ew, and u he esea ch is equi ed.
P ope usion o he di e en localiza ion algo i hms o allow o e coming he limi a ions
o each indi idual senso in di e en scena ios, including he possibili y o de ec ing e o s
while in ope a ion, and econ igu ing he localiza ion amewo k o compensa e o senso
ailu e o deg ada ion a e some o he a eas ha ha e no been analyzed in he li e a u e.
The p esen wo k aims o p opose con ibu ions in hese a eas, as de ailed nex .
Senso s 2022,22, 2595 4 o 16
1.3. Con ibu ions
In his wo k, a econ igu able localiza ion amewo k is p oposed, sui able o dense
u ban a eas and capable o de ec ing posi ioning e o s and econ igu ing aul y modules
acco dingly o ensu e co ec and accu a e localiza ion. The p oposed amewo k is an
ex ension o he one p esen ed by he au ho s in [
37
] and p esen s he ollowing main
con ibu ions o e he wo ks p e iously analyzed: (1) a localiza ion amewo k able o
cope wi h di e en senso ailu es hanks o he combina ion o usion algo i hms and
decision s a egies; (2) he de ini ion wi hin he amewo k o a senso -measu emen -based
e o -de ec ion s a egy, which allows de e mina ion o which senso is ailing so ha
he da a p o ided by he ailing senso can be neglec ed; and (3) a no el econ igu a ion
module ha e alua es he ailu e scena io and econ igu es he sys em, adop ing al e na i e
localiza ion s a egies ha use emaining senso da a o a oid ehicle s op un il he sys em
is ully deg aded.
The p oposed econ igu able localiza ion amewo k is alida ed in di e en simula ed
u ban scena ios agains a wide ange o possible e o s using a CARLA simula o . In his
alida ion, i is o be no ed ha in his wo k no new algo i hms a e p oposed o ision,
LiDAR o INS/GNSS da a p ocessing, as he aim o his wo k and he alida ion is no o
achie e maximum accu acy. Ins ead, c i ical e o s a e simula ed in he senso y sys em,
and alida ion is ocused on he e o -de ec ion and econ igu a ion s a egy p oposed in
o de o demons a e ha he sys em is able o u he enhance ope a ion o he loca ion
sys em by p o iding he bes possible localiza ion.
The emainde o he pape is o ganized as ollows. The localiza ion amewo k is
de ined in Sec ion 2, including he e o -de ec ion and econ igu a ion s a egy. Sec ion 3
de ails he simula ion p ocess o es he iabili y o he localiza ion sys em in he p esence
o e o s. Finally, he main ideas o his wo k a e summa ized in Sec ion 4.
2. Recon igu able Localiza ion F amewo k
The p oposed econ igu able localiza ion amewo k is based on a hie a chical lo-
caliza ion algo i hm s uc u e, an e o -de ec ion block and he econ igu a ion s a egy
implemen ed in a decision block. The hie a chical s uc u e is composed o h ee le els,
each wi h a di e en localiza ion app oach, o de ed om mos o leas accu a e: Le el
3 uses algo i hms based on calcula ion o he ela i e posi ion, using LiDAR, monocula
came a, ine ial measu emen uni (IMU) and odome y; Le el 2, based on he p e ious
es ima ions, adds a digi al map o pe o m oad-ma ching; inally, Le el 1 is based on
GPS/INS localiza ion. The e o -de ec ion block con inuously moni o s he accu acy o
each localiza ion algo i hm, p o iding his in o ma ion o a decision block ha is esponsi-
ble o de ining which le el o he hie a chical s uc u e is going o be used. This amewo k
assumes ha a low-le el lane keeping con ol and c uise con ol a e implemen ed in he
ego ehicle o assu e ha he ehicle always emains wi hin he bounda ies o he oad
despi e ha ing a ailu e in he global localiza ion modules.
Figu e 1summa izes he p oposed localiza ion amewo k, including all h ee hie a -
chical le els, he ou pu -selec ion-decision block and he e o -de ec ion block. All o hese
elemen s will be discussed in dep h wi hin he nex subsec ions.
Senso s 2022,22, 2595 5 o 16
Figu e 1. P oposed localiza ion amewo k.
2.1. Le el 3: Rela i e Posi ioning S a egies
This le el p ocesses senso da a using app oaches ocused on ela i e-localiza ion
algo i hms. Fou di e en algo i hms a e implemen ed in his le el, ela ed o each o
he ou senso ou pu s conside ed: came a, LiDAR, IMU and odome y. Fo he i s ,
a VO echnique is applied based on he one p oposed in [
12
]. Fo he LiDAR senso , a
LiDAR odome y s a egy is applied, based on NDT cloud-poin ma ching o a egis a ion
algo i hm [
38
] be ween consecu i e ime-s eps. Accele ome e and gy oscope eadings
om he IMU a e combined in an ex ended Kalman il e (EKF) based in measu emen
in eg a ion [
39
]. Finally, he odome y eadings, consis ing o he ehicle’s wheel speed
and s ee ing angle, a e used o eed a bicycle model o he ego ehicle [
40
] and es ima e
he loca ion o he ehicle by in eg a ion [5].
The p e ious app oaches a e based on compu ing he ela i e displacemen and
ehicle’s yaw-angle a ia ion be ween consecu i e ime-s eps. Hence, o es ima e he
localiza ion o he ego ehicle
xi
, an ini ial known posi ion and o ien a ion om he
p e ious ime s ep mus be p o ided, xi−1and φi−1, espec i ely:
xi
yi=d·cos(φi)
sin(φi)+xi−1
yi−1(1)
φi=∆φ+φi−1(2)
whe e dde ines he es ima ed dis ance a ia ion be ween wo s eps.
In o de o p o ide a mo e obus ou pu , he ou algo i hms p e iously de ined a e
used using an ex ended Kalman il e , which es ima es he ego ehicle posi ion
xL3
and
o ien a ion alues
φL3
. These wo alues will be p opaga ed o he nex le el, becoming he
s a ing es ima ion o he map-ma ching algo i hm.
As p e iously de ailed, algo i hms based on ela i e posi ioning p esen d i o e
ime due o he accumula ion o small e o s in he calcula ed ela i e displacemen s.
The e o e, i he decision block selec s only his le el as he ou pu o he localiza ion
amewo k, he sys em will be conside ed in a c i ical s a e, as i is no sui able o long-
e m accu a e na iga ion.
2.2. Le el 2: Road-Ma ching Algo i hms
This le el combines he es ima ion p o ided by Le el 3 wi h a geome y-based, poin -
o-poin , oad-ma ching algo i hm [
10
], p o iding mo e accu a e global localiza ion es ima-
ions
xL2
and
φL2
. Besides ha , an HD map, gi en by CARLA, is used as an ex a inpu
o senso o he localiza ion ask, simila o he mos common oad-ma ching algo i hms.
Howe e , oad-ma ching algo i hms p esen wo main issues ha comp omise hei accu-
acy: misma ches in junc ions, mainly whe e many oads come oge he , and longi udinal
d i , which may accumula e o e ime despi e co ec ly ma ching he oad link. These
e o s may lead o an inco ec link selec ion by he oad-ma ching algo i hm.
Senso s 2022,22, 2595 6 o 16
To deal wi h hese issues, he p oposed wo k simula es a simple ea u e-based co ec-
ion algo i hm, simila o [
13
], o de ec oad junc ions and ma ch hem wi h hei posi ion
on he digi al map. This way, e e y ime he ehicle eaches a junc ion, he p e ious
posi ion es ima ion can be co ec ed based on he global posi ion o he de ec ed junc ion.
I should be unde lined ha his me hod assumes ha he junc ion-de ec ion algo i hm
always wo ks co ec ly, based on he p emise ha an au onomous ehicle canno ollow i s
ajec o y and make decisions acco dingly wi hou p ope ly unc ioning junc ion de ec ion.
Le el 2 localiza ion can p o ide app op ia e global posi ion and o ien a ion in o -
ma ion o a limi ed ime, as he p oposed app oach will p esen longi udinal d i , he
co ec ion o which will depend on he de ec ion o he a o emen ioned ea u e (junc ions).
The e o e, a localiza ion sys em wo king a Le el 2 will be conside ed deg aded.
2.3. Le el 1: GPS/INS
Le el 1 implemen s he mos accu a e global localiza ion app oach on his amewo k,
based on he widely used GPS/INS localiza ion app oach ha combines GPS da a wi h
INS senso da a using an in eg a ed Kalman il e [
2
]. I is assumed ha his signal equi es
no u he compu a ion o es ima e ehicle posi ion,
xL1
. This localiza ion echnique is he
mos accu a e one since i is no suscep ible o ex e nal ac o s such as ehicle pa ame e
misma ches o accumula ed d i , al hough his does no always ensu e absolu e accu acy.
Mo eo e , i s only e o sou ce comes om i s own senso - eading p ecision and he
ex e nal signal i ecei es om sa elli es.
The e o e, assuming ha in an ideal si ua ion he eadings o he GPS/INS senso will
p o ide he mos accu a e es ima ion, his le el will always ha e he op p io i y in he
localiza ion s uc u e. Hence, a localiza ion sys em wo king a Le el 1 will be conside ed
as no mal beha io , as i is sui able o ully accu a e long- e m na iga ion.
2.4. E o -De ec ion Block
The e o -de ec ion block is esponsible o e alua ing he es ima ion p o ided by
he a o emen ioned h ee le els and de e mining i he e is a ailu e in one o he le els
by modi ying he e o s a e a iable, which will la e be p ocessed by he decision block o
econ igu e he ou pu o he localiza ion sys em.
The e o -de ec ion block has been designed o conside he ollowing ailu es
and mal unc ions:
•
Le el 1: he GPS/INS-based localiza ion le el’s accu acy is mainly a ec ed by he
sa elli e signal. The e o e, i s mal unc ion will always come om obs uc ion (quali y
deg ada ion) o lack o his signal.
•
Le el 2: simila o he Le el 1 e o sou ce, map-ma ching algo i hm accu acy is
s ongly linked o he digi al map p ecision and a ailabili y. The e o e, in case he
ehicle en e s an a ea whe e he e is no map da a o i is no accu a e, he map-
ma ching algo i hm will be agged as inaccu a e.
•
Le el 3: senso s may become aul y because o wea , ailu e o a change in he
en i onmen ha a ec s he na u e o hei eadings. Some examples o possible en i-
onmen al changes a e: low isibili y du ing nigh o wiligh o came as, p esence o
ligh ning o s ong lashes o he LiDAR o i e slippage o ehicle odome y and
INS eadings.
In o de o p o ide a obus localiza ion app oach in he e o -de ec ion block, s a e-
gies o de ec he a o emen ioned e ec s a e implemen ed. Fo ha pu pose, wo main
app oaches a e ollowed, depending on he hie a chical le el analyzed. Fo Le els 1 and 2,
wo con ol signals ha e been implemen ed o e alua e GPS signal quali y,
u1
, o map
p ecision,
u2
. No e ha GPS de ices ypically p o ide his in o ma ion based on sa elli e
signal s eng h and he numbe o de ec ed sa elli es. In he case o digi al maps, e o can
be easily de ec ed by he ehicle mo ing in o an uncha ed a ea no de ined on he map.
On he o he hand, he algo i hms in eg a ed in o Le el 3 di ec ly use each senso
es ima ion da a, bo h o posi ion and o ien a ion. Hence, o his le el, a con lic e alua ion
Senso s 2022,22, 2595 7 o 16
app oach has been de eloped o de ec ailu e o one o he senso s (see Figu e 2). The
p oposed algo i hm is based on he one p oposed in [
41
] and uses a duplica ion-compa ison
echnique ha compu es a dynamic eliabili y alue o each sou ce. The p oposed ap-
p oach can de ec aul y senso s by compa ing he con lic o disc epancy be ween he
ou pu es ima ions p o ided by each senso (and i s co esponding p ocessing algo i hms,
as de ailed in Sec ion 2.1) using he ollowing equa ion:
csenso i=1
(n−1)∗
n−1
∑
k=1xi
φi−xk
φk(3)
whe e
xi
and
φi
ep esen he posi ion and o ien a ion es ima ion o he senso whose
con lic is being e alua ed, p io o he Le el 3 EKF il e usion;
xk
and
φk
a e he po-
si ion and o ien a ion o he es o he senso s;
n
is he numbe o senso s in eg a ed
in he amewo k ( ou in his app oach) and
csenso i
is he con lic alue associa ed wi h
each senso .
Figu e 2. Con lic algo i hm s uc u e.
A e calcula ing he con lic alue
csenso i
, i is compa ed wi h a h eshold alue
csenso i
, which is se expe imen ally as 2.5 m o he posi ion es ima ions and 5 deg ees o
he o ien a ion es ima ion. I his alue is su passed, a dynamic eliabili y alue
σdini
is
calcula ed as ollows:
σdini=0csenso i≤csenso
σdini=csenso i+ ·eicsenso i>csenso (4)
which de ines he dynamic eliabili y alue p opo ion
o he maximum de ia ion o he
e alua ed es ima ion om he es o he es ima ions
ei
. Va iable
is se empi ically o 2.5
o he o ien a ion es ima ion and 1 o he posi ion es ima ion. The dynamic eliabili y
alue is added o he s a ic eliabili y o each sou ce
σes i
, which cha ac e izes he expec ed
accu acy o each senso –algo i hm pai and is based on each algo i hm’s expec ed accu acy
as ound in he li e a u e, allowing ob ainmen o he global eliabili y σi:
σi=σdini+σes i.(5)
Finally, his las alue is ans o med in o he measu emen noise co a iance ma ix
Rias ollows:
Ri=σi0
0σi. (6)
This alue is in oduced in o he EKF used o use he di e en ou pu s in Le el 3.
This way, whene e one o he sou ces becomes aul y, i s dynamic eliabili y alue will
g ow and u he p opaga e o he co a iance ma ix, making he EKF lowe he weigh
o his same sou ce in he inal ou pu , elimina ing o mi iga ing he e o in luence. The
Senso s 2022,22, 2595 8 o 16
main ad an age o his app oach is he ins an de ec ion o mal unc ioning sys ems, as i s
de ec ion is based on immedia e measu emen s and no s a is ical me ics.
Figu e 2depic s he s uc u e o he implemen ed con lic algo i hm o Le el 3, along
wi h he global eliabili y ans o ma ion in o an EKF measu emen noise co a iance ma ix
o each sou ce.
No e ha he p oposed con lic algo i hm needs a minimum o h ee di e en sou ces
o de ec a single aul y sou ce [
42
]. Thus, he e is a limi on he numbe o senso ailu es
ha he con lic e alua ion sys em will be able o de ec . Whene e his limi is exceeded,
con lic be ween all senso s, bo h he ailing and co ec ly wo king ones, g ows abo e he
h eshold, and he sys em canno be us ed anymo e.
2.5. Decision Block
This decision o econ igu a ion block ecei es he es ima ions calcula ed in he h ee
Le els and he e o s a e o each le el, cha ac e ized by he a iable
e o s a e
, and de ines
he ou pu o he localiza ion amewo k and i s s a e (no mal, deg aded, c i ical o eme -
gency). The beha io o his block is summa ized in Table 1. No e ha his able illus a es
he ailu es ha equi e econ igu a ion o he localiza ion amewo k’s ou pu .
I is o be no ed ha , as p e iously de ailed, econ igu ing he amewo k’s ou pu o
he es ima ion p o ided by Le el 2 o Le el 3 does no ensu e long- e m accu acy. Hence,
ime h esholds a e included o he deg aded and c i ical cases. Fo he i s , he ime
h eshold de ines he maximum ime he Le el 2 algo i hm can sa ely ope a e wi hou
ese ing he longi udinal d i using he junc ion-de ec ion algo i hms. A wa chdog ime
is ini ialized when en e ing he deg aded s a e and ese s each ime he longi udinal
d i is co ec ed. Howe e , i his co ec ion is no possible, an eme gency s op signal
will be gene a ed (ma ked wi h *). Besides ha , a co ec ion dis ance h eshold is also
implemen ed in Le el 2. Once a junc ion is de ec ed (based on ea u e de ec ion), he closes
junc ion posi ion on he digi al map will be sea ched. An eme gency s op signal will also
be gene a ed, i no junc ion is ound wi hin a con idence dis ance alue.
Fo he case o Le el 3, e en i he in e nal EKF ensu es he bes possible localiza ion
accu acy, a ime h eshold is de ined in which he ehicle should pe o m a allback s a egy.
I he ime is eached, a eme gency s op signal will be gene a ed. Fu he mo e, i he Le el
3 localiza ion sys em e e accumula es oo many aul y senso s o be us ed, he decision
block will send an eme gency signal o ind a sa e s op o he ehicle.
Table 1.
Decision block s a egies. The as e isk indica es ha an eme gency s op signal is gene a ed.
Failu es Local.
Ou pu S a e Eme g.
S op?
None Le el 1 No mal
beha iou No
Le el 1 Le el 2 Deg aded Yes *
Le el 1
Le el 2 Le el 3 C i ical Yes *
Le el 1
Le el 2
Mo e han 2 aul y senso s a Le el 3
None Eme gency Yes
One o he main con ibu ions o his wo k is he concep o econ igu a ion be ween
he di e en Le els, which allows handling o sho ailu es, such signal loss o in e mi en
ailu es, allowing con inued ope a ion o he ehicle. A clea example o hese ailu es
a e GPS signal losses when d i ing h ough dense u ban a eas, as sa elli e signals a e los
and eco e ed cons an ly. In hese cases, he p oposed app oach akes ad an age o he
accu acy o highe le els o include a d i ese o lowe ones.
Senso s 2022,22, 2595 9 o 16
3. Valida ion
In his sec ion he p oposed localiza ion amewo k will be es ed and alida ed in a
ealis ic simula ion en i onmen in which di e en ailu e scena ios will be e alua ed.
3.1. Simula ion Se up
In o de o alida e he p oposed app oach, a se o simula ed scena ios ha e been
implemen ed in he CARLA Simula o en i onmen , pa icula ly in he cus om scena io
“Town 03” [
43
]. A Tesla 3 ehicle has been selec ed as he s udy case, equipped wi h
GPS/INS, LiDAR, monocula came a, IMU and wheel speed and s ee ing angle odome y
senso s. Da a om CARLA is ans e ed using he Py hon API o Ma lab/Simulink, whe e
he p oposed localiza ion app oach has been implemen ed. Rega ding he eme gency
signal con ol wi hin he decision block, o Le el 2 a 30 s ime h eshold and a 10 m
dis ance h eshold ha e been implemen ed, and o Le el 3, a 15 s h eshold.
Twel e di e en scena ios a e p oposed o alida e he localiza ion amewo k. All
scena ios a y in hei ou es, bu hey sha e he same e o pa e n, lis ed in Table 2. As
can be seen om he able, Le el 1 and 2 ailu es a e in e mi en wi h a ying eco e y
imes. On he o he hand, Le el 3 senso ailu es a e no eco e ed: a e 1 min he LiDAR
measu emen s s a o ail and a e 2 min odome y will become un eliable, which will
pu he localiza ion sys em jus wi hin he limi o i s unc ionali y du ing he las c i ical
s a e. All hese ailu es a e simula ed by manually dis up ing he CARLA senso modules
once he eadings ha e been ecei ed. The dis up ion magni udes a e simula ed acco ding
o he li e a u e and he eco ded e o s o each senso [
2
,
12
,
44
]. This way, he se up
aims o es he p oposed app oach in he p esence o a se o longe e o s—si ua ions in
which g aph-based op imiza ion o o he EKF s a egies ail. The main objec i e o his
simula ion is o alida e he obus ness o he localiza ion amewo k, he con lic -based
e o -de ec ion s a egy and he d i - ese s a egy o lowe le els.
Table 2. E o pa e n o 12 scena ios.
Simula ion Time (s) Failu e Reco e y Time (s) Du a ion (s)
30 Le el 1 los 70 40
60 Le el 2 los 66 6
60 IMU los - -
72 Le el 1 los 100 28
90 Le el 2 los 96 6
106 Le el 2 los 112 6
120 LiDAR los - -
120 Le el 1 los 170 50
160 Le el 2 los 172 12
3.2. Simula ion Resul s
Table 3summa izes he simula ion esul s o scena ios ha comple e hei ajec o ies,
i.e., scena ios whe e he decision block does no send an eme gency signal o s op he
ehicle. To e alua e hese esul s a mean-e o -based c i e ia is applied, based on he
esul s o simila wo ks in his a ea [
31
,
32
]. S ill, i should be unde lined ha he e e enced
wo ks’ app oaches do no conside senso ailu es in hei esul s. The e o e, based on hese
wo ks, a mean e o highe han 3 m in he longi udinal and 1 m in he la e al posi ioning
a e se as c i e ia o de e mine he scena io o a mal unc ion. This is also he eason o
no including addi ional s a is ical measu es o he pe o mance, such as maximum e o
alues o CDF g aphs. The p esen ed amewo k is ocused on gua an eeing ha in he
p esence o deg aded si ua ions localiza ion can be es ima ed un il a c i ical ailu e a ises,
and ha he sys em is able o econ igu e i sel and co ec he posi ion es ima ion despi e
he appea ance o peak e o alues.
Senso s 2022,22, 2595 16 o 16
24.
Ala ise, M.B.; Hancke, G.P. A Re iew on Challenges o Au onomous Mobile Robo and Senso Fusion Me hods. IEEE Access
2020
,
8, 39830–39846. [C ossRe ]
25.
Wang, K.; Ma, S.; Ren, F.; Lu, J. SBAS: Salien Bundle Adjus men o Visual SLAM. SBAS Salien Bundle Adjus . Vis. SLAM
2021
,
70, 1–9. [C ossRe ]
26.
Li, X.; Xu, Q. A Reliable Fusion Posi ioning S a egy o Land Vehicles in GPS-Denied En i onmen s Based on Low-Cos Senso s.
IEEE T ans. Ind. Elec on. 2017,64, 3205–3215. [C ossRe ]
27.
Sme s, P. Da a usion in he ans e able Belie Model. In P oceedings o he 3 d In e na ional Con e ence on In o ma ion Fusion,
Pa is, F ance, 10–13 July 2000; Volume 1, pp. 21–33.
28.
G ubmülle , S.; S e inge , G.; So elo, M.A.; Wa zenig, D. Faul - ole an en i onmen al pe cep ion a chi ec u e o obus
au oma ed d i ing. In P oceedings o he IEEE In e na ional Con e ence on Connec ed Vehicles and Expo (ICCVE), G az, Aus ia,
4–8 No embe 2019; pp. 1–6.
29.
Kaci, B.; Benjamin, L.; Wal e , S. A Faul Tole an A chi ec u e o Da a Fusion Ta ge ing Ha dwa e and So wa e Faul s. In
P oceedings o he IEEE 20 h Paci ic Rim In e na ional Symposium on Dependable Compu ing, Singapo e, 18–21 No embe 2014;
pp. 1–10.
30. Chen, Z.; Nisa , A.; Julie , S.; Heckman, C. Kalman Fil e Tuning wi h Bayesian Op imiza ion. a Xi 2019, a Xi :1912.08601.
31.
Chen, Z.; Heckman, C.; Julie , S.; Nisa , A. Weak in he NEES? Au o-Tuning Kalman Fil e s wi h Bayesian Op imiza ion. In
P oceedings o he 2018 In e na ional Con e ence on In o ma ion Fusion (FUSION), Camb idge, UK, 10–13 July 2018; pp. 1072–1079.
[C ossRe ]
32.
G iebel, T.; Mülle , J.; Buchholz, M.; Die maye , K. Kalman Fil e Mee s Subjec i e Logic: A Sel -Assessing Kalman Fil e Using
Subjec i e Logic. In P oceedings o he IEEE 23 d In e na ional Con e ence on In o ma ion Fusion (FUSION), Rus enbu g, Sou h
A ica, 6–9 July 2020.
33.
Bingyi, C.; Ri e , C.N.; Göh ing, D.; Rojas, R. Accu a e Localiza ion o Au onomous Vehicles Based on Pa e n Ma ching and
G aph-Based Op imiza ion in U ban En i onmen s. In P oceedings o he 2020 IEEE 23 d In e na ional Con e ence on In elligen
T anspo a ion Sys ems (ITSC), Rhodes, G eece, 20–23 Sep embe 2020, pp. 1–6.
34.
Xue, W.; Yang, B.; Kaizuka, T.; Nakano, K. A Fallback App oach o an Au oma ed Vehicle Encoun e ing Senso Failu e in
Moni o ing En i onmen . In P oceedings o he 2018 IEEE In elligen Vehicles Symposium (IV), Changshu, China, 26–30 June
2018; pp. 1807–1812. [C ossRe ]
35.
Ma u e-Peaspan, J.A.; Pe ez, J.; Zubiza e a, A. A Fail-Ope a ional Con ol A chi ec u e App oach and Dead-Reckoning S a egy
in Case o Posi ioning Failu es. Senso s 2020,20, 442. [C ossRe ]
36.
Yu, J.; Luo, F. Fallback S a egy o Le el 4+ Au oma ed D i ing Sys em. In P oceedings o he 2019 IEEE In elligen T anspo a ion
Sys ems Con e ence (ITSC), Auckland, New Zealand, 27–30 Oc obe 2019, pp. 156–162. [C ossRe ]
37.
Viana, K.; Zubiza e a, A.; Díez, M. Robus localiza ion o au onomous ehicles in dense u ban a eas. In P oceedings o he
2021 25 h In e na ional Con e ence on Sys em Theo y, Con ol and Compu ing (ICSTCC), Iasi, Romania, 20–23 Oc obe 2021;
pp. 107–112.
38.
Magnusson, M. The Th ee-Dimensional No mal-Dis ibu ions T ans o m—An E icien Rep esen a ion o Regis a ion, Su ace
Analysis, and Loop De ec ion. Ph.D. Thesis, Ö eb o Uni e si y, Ö eb o, Sweden, 2013.
39.
Ala ise, M.B.; Hancke, G.P. Pose Es ima ion o a Mobile Robo Based on Fusion o IMU Da a and Vision Da a Using an Ex ended
Kalman Fil e . Senso s 2017,17, 2164. [C ossRe ]
40.
Ma u e, J.A.; Ma cano, M.; Diaz, S.; Pe ez, J. Expe imen al Valida ion o a Kinema ic Bicycle Model P edic i e Con ol wi h
La e al Accele a ion Conside a ion. IFAC-Pape sOnLine 2019,52, 289–294. [C ossRe ]
41.
Delmo e, F.; Gacque , G. De ec ion o de ec i e sou ces wi h belie unc ions. In P oceedings o he 2 h In e na ional Con e ence
on In o ma ion P ocessing and Managemen o Unce ain y in Knowledge-Based Sys ems, Lisbon, Po ugal, 15–19 June 2008;
pp. 337–344.
42.
Ricquebou g, V.; Dela osse, M.; Delahoche, L.; Ma hic, B.; Jolly-Desod , A.; Menga, D. Faul De ec ion by Combining Redundan
Senso s: A Con lic App oach wi hin he TBM F amewo k. In P oceedings o he COGni i e Sys ems wi h In e ac i e Senso s
(COGIS), Camb idge, MA, USA, 25–27 Ap il 2007.
43.
Doso i skiy, A.; Ros, G.; Code illa, F.; Lopez, A.; Kol un, V. CARLA: An Open U ban D i ing Simula o . In P oceedings o he
1s Annual Con e ence on Robo Lea ning, Moun ain View, CA, USA, 13–15 No embe 2017; pp. 1–16.
44.
Daza, I.; Ren e o, M.; Maldonado, C.; Izquie do, R.; He nández, N.; Balla dini, A.; Fe nández-Llo ca, D. Fail-Awa e LIDAR-Based
Odome y o Au onomous Vehicles. Senso s 2020,20, 4097. [C ossRe ] [PubMed]
