A Low-Cost Consistent Vehicle Localization Based on ...downloads.hindawi.com/journals/jat/2018/2713729.pdf · ResearchArticle A Low-Cost Consistent Vehicle Localization Based on Interval

Research ArticleA Low-Cost Consistent Vehicle Localization Based onInterval Constraint Propagation

Zhan Wang and Alain Lambert

Laboratoire de Recherche en Informatique (LRI) CNRS Univ Paris-Sud Universite Paris-Saclay 91403 Orsay France

Correspondence should be addressed to Alain Lambert alainlambertu-psudfr

Received 10 March 2018 Revised 3 May 2018 Accepted 13 May 2018 Published 24 June 2018

Academic Editor Krzysztof Okarma

Copyright copy 2018 Zhan Wang and Alain Lambert This is an open access article distributed under the Creative CommonsAttribution License which permits unrestricted use distribution and reproduction in any medium provided the original work isproperly cited

Probabilistic techniques (such as Extended Kalman Filter and Particle Filter) have long been used to solve robotic localizationand mapping problem Despite their good performance in practical applications they could suffer inconsistency problems Thispaper proposes an interval analysis based method to estimate the vehicle pose (position and orientation) in a consistent way byfusing low-cost sensors and map data We cast the localization problem into an Interval Constraint Satisfaction Problem (ICSP)solved via Interval Constraint Propagation (ICP) techniques An interval map is built when a vehicle embedding expensive sensorsnavigates around the environmentThen vehicles with low-cost sensors (dead reckoning and monocular camera) can use this mapfor ego-localization Experimental results show the soundness of the proposed method in achieving consistent localization

1 Introduction

Using sensory information to locate the robot in its environ-ment is one of the most fundamental problem providing amobile robot with autonomous capabilities A vast numberof works [1 2] dedicated to such problems use both propri-oceptive and exteroceptive sensors Proprioceptive sensorssuch as odometers and gyrometers are used to calculate theelementary movements which are used to estimate the robotpose (position and orientation) However this generatescumulative errors as the robot moves [3 4] To cope withthis problem exteroceptive sensors (lasers sonars GPScameras etc) are used to gather information either from thesurrounding environment or from a global reference aimingat eliminating the accumulated errors and improving theestimation of the robot pose [5 6]

Probabilistic techniques have long been used in solv-ing robotic localization and mapping problem The mostcommonly used methods are Kalman filtering and Particlefiltering [7 8] These methods associate a probability to aposition but nothing ensures that the robot is indeed in theposition with the highest probability It has long been noticedby the research community that probabilistic methods suffer

the inconsistency problem [9] It is due to the fact thatthey are based on the assumption that the sensor noises aredescribed by probability density function However in realworld many sensors do not possess Gaussian noise model ortheir distributions are simply not available [10]

Interval analysis [11] is an alternative and less knownmethod which allows us to solve nonlinear problems ina guaranteed way Instead of making hypothesis on theprobability distribution of sensor noises interval methodstake the assumption that all the noises are bounded withinknown limits This seems to be a realistic representationas most sensor manufacturers provide the maximum andminimum measurement errors under suitable working con-ditions These extreme values of error can then be regardedas the error bounds Interval based algorithms can be usedto recursively propagate such bounded errors by using con-sistency techniques and systematic searchmethods Contraryto probability methods interval methods provide guaranteedsets which enclose the real state without losing any feasiblevalue

Interval methods have achieved promising results inparameter and state estimation tasks [12ndash14] and they foundtheir way to the mobile robotic localization and mapping

HindawiJournal of Advanced TransportationVolume 2018 Article ID 2713729 15 pageshttpsdoiorg10115520182713729

2 Journal of Advanced Transportation

area Instead of considering Gaussian noise all model andmeasurement noises are assumed to be bounded The aimis to guarantee the configurations of the robot pose that areconsistent with the given measurements and noise boundsKieffer [15] and Jaulin [16] provided simulation results for arobot equipped with a belt of on-board ultrasonic sensorsThe feasibility of interval methods on localization appli-cations had been shown Followed-up researchers focusedon real-time implementation of such localization techniqueKieffer [17] and Seignez [18] presented results for a mobilerobot navigating in an indoor environment with odometersmounted on each rear wheel to measure the motion andsonars located in different directions to perceive the environ-ment The ability of interval method to cope with erroneousdata and to obtain accurate estimations of the robot posewas demonstrated Lambert [19] extended such work byproviding results for an outdoor vehicle equipped withodometers gyro and GPS receiver Comparison was madewith the Particle Filter localization showing the better per-formance of interval method in terms of consistency Gning[2] and Kueviakoe [20] dealt with the outdoor localizationproblem in the framework of Interval Constraint SatisfactionProblem (ICSP) Those works used constraint propagationtechniques to fuse the redundant data of sensors Drevelle[21] uses relaxed constraint propagation approach to dealwith erroneous GPSmeasurements Bonnifait [22] combinedconstraint propagation and set inversion techniques andpresented a cooperative localization method with significantenhancement in terms of accuracy and confidence domainsExperimental results illustrate that constraint propagationtechniques are well adapted to real-time implementationand provide consistent result for localization Some otherresearchers proposed specific contractors [23] or separators[24] to solve the localization problem

Indeed when considering consistency issue intervalmethods are advantageous in vehicle localization Howeverthey are prone to large output imprecision which can makethem unsuitable for high level task We improve on itand present an extension of othersrsquo works dealing with thelocalization problem by using ICP Instead of using ultrasonicsensors [16] sonar [18 25] or GPS [19 26] as exteroceptivesensors we propose to use a monocular camera The aim isto achieve both consistent and low imprecision localizationresult We propose to drive a vehicle embedding precisebut expensive sensors around the environment in orderto build up a map represented in an interval way Thenthe proposed method enables a set or a single vehicle tonavigate consistently in such environment using this mapand low-cost sensors By constructing the problem as anICSP the proposed method is able to handle both mappingand localization in a unified form using Interval ConstraintPropagation techniques

The paper is organized as follows Section 2 introducesthe basics of interval analysis Section 3 illustrates how themapping and localization problem can be formulated as anInterval Constraint Satisfaction Problem Sections 4 and 5present the simulation and experimental results in terms ofmapping and localization Section 6 draws a conclusion of thepaper

2 Overview of Interval Analysis andConstraint Propagation

Interval analysis and constraint propagation are the the-oretical tools used in interval based methods for roboticlocalization and mapping In this section we give a briefoverview of these tools

21 Interval Analysis Interval analysis [11 27] is a numericalmethod which allows us to solve nonlinear problems in aguaranteed way One of the pioneers of interval analysisRamon E Moore proposed to represent a solution of a prob-lem by an interval in which the real solution is guaranteedAn interval [119909] is a connected subset ofR defined by its lowerbound 119909 and upper bound 119909[119909] = [119909 119909] = 119909 isin R | 119909 le 119909 le 119909 (1)

The width of a nonempty interval [119909] is calculated by119908([119909]) = 119909 minus 119909 and the midpoint (or center) is 119898119894119889([119909]) =(119909 minus 119909)2 If [119909] is an interval containing the 119909 position ofa vehicle 119898119894119889([119909]) can be taken as an estimation of 119909 and119908([119909])2 can be regarded as the estimation error [28]

Let IR be the set of the intervals of R A 119887119900119909 (alsocalled 119894119899119905119890119903V119886119897 V119890119888119905119900119903) [x] ([x] isin IR119899) is a generalizationof the interval concept It is a vector whose components areintervals [x] = [1199091] times [1199092] times sdot sdot sdot times [119909119899] (2)

For instance the configuration of a vehiclersquos pose usuallycontains 3 parameters position 119909 position 119910 and headingangle 120579 Consequently for vehicle localization the solution isa three-dimensional box [119909] times [119910] times [120579]

The width of a nonempty box [x] can be computed by119908([x]) = max119894=1119899119908([119909119894]) We define the volume of [x] asV119900119897 ([x]) = prod

1le119894le119899

119908 ([119909119894]) (3)

The volume of the box is usually used to evaluate theestimation uncertainty of a state vector [28]

22 Operations of Interval Arithmetic Rules have beendefined to apply the basic arithmetical operations on intervals[27] Let us consider two intervals [119909] and [119910] and a binaryoperator isin + minus times divide the smallest interval which containsall feasible values for [119909] [119910] is defined as follows[119909] [119910] = 119909 119910 | 119909 isin [119909] 119910 isin [119910] (4)

Example 1 Addition [minus4 3] + [2 5] = [minus2 8] subtraction[3 5] minus [1 2] = [1 4] multiplication [minus1 8] times [minus3 1] =[minus24 8] division [minus2 8] divide [2 4] = [minus1 4]The set-theoretic operations can be applied to intervals

The 119894119899119905119890119903119904119890119888119905119894119900119899 of two intervals is defined by[119909] cap [119910] = 119911 isin R | 119911 isin [119909] and 119911 isin [119910] (5)

Journal of Advanced Transportation 3

[x] [x]

[x] cap [y] [x] ⊔ [y]

[y] [y]

Figure 1 Intersection and interval hull of interval boxes

[x]

f([x])

f[]([x])

IRm IRn

Figure 2 Images of an interval box by function 119891 and its inclusionfunction 119891[]The result of 119894119899119905119890119903119904119890119888119905119894119900119899 is always an interval but it is not thecase for their 119906119899119894119900119899[119909] cup [119910] = 119911 isin R | 119911 isin [119909] or 119911 isin [119910] (6)

Consequently an 119894119899119905119890119903V119886119897 ℎ119906119897119897 is defined as the smallestinterval that contains all the subsets of [119909] cup [119910] denoted by[119909] ⊔ [119910] [119909] ⊔ [119910] = [[119909] cup [119910]] (7)

For instance the interval hull of [2 4] cup [5 8] is the interval[2 8] Figure 1 gives an example of 119894119899119905119890119903s119890119888119905119894119900119899 and 119906119899119894119900119899 for2 interval boxes it is realized by applying the intersection andinterval hull operator to each component of the box

23 Inclusion Function The image of a vector function 119891 IR119898 rarr IR119899 (defined by arithmetical operators and elemen-tary functions) over an interval box [x] can be evaluated byits inclusion function 119891[] whose output contains all possiblevalues taken by 119891(sdot) over [x] (see Figure 2)forall [x] isin IR

119898 119891 ([x]) sub 119891[] ([x]) (8)

The simplest way to create an inclusion function is to replaceall the variables and operators by their interval equivalentsThe resulting interval function is called ldquonatural inclusionfunctionrdquo For instanceforall119909 isin R acircŊĹ119891 (119909) = 41199094 minus 7ln (119909) minus 3 (9)

has the natural inclusion functionforall [119909] isin IR 119891[] ([119909]) = 4 [119909]4 minus 7ln[] ([119909]) minus 3 (10)

Inclusion function can also be used to represent equationsdefined by interval variables Such equations also calledconstraints are the core of Interval Constraint SatisfactionProblems

Constraint

[x]

c(x)

([x])

Figure 3 Contraction of a box by a contractorC

24 Contractor Theconcept of contractor is directly inspiredfrom the ubiquitous concept of filtering algorithms in con-straint programming Given a constraint 119888 relating a set ofvariables119909 an algorithmC is called a contractor (or a filteringalgorithm) ifC returns a subdomain of the input domain [x]and the resulting subdomain C([x]) contains all the feasiblepoints (the gray domain in Figure 3) with respect to theconstraint 119888

C ([x]) sube [x] and forall119909 isin [x] 119888 (119909) 997904rArr 119909 isin C ([x]) (11)

25 Interval Constraint Satisfaction Problem (ICSP) Theconcept of Interval Constraint Satisfaction Problem (ICSP)was introduced by Hyvonen [29] in 1992 It is typicallydefined as

(i) a set 119881 of variables V1 V2 V119899(ii) a set 119863 of domains 1198891 1198892 119889119899 such that for each

variable V119894 a domain 119889119894 with the possible values forthat variable is given 119889119894 is an interval or union ofintervals

(iii) a set 119862 of 119901 constraints 1198881 1198882 119888119901 each constraint119888119894 defines the relationships of a number of variablesfrom 119881 eg 1198881(V1 V2 V3) = 0 restricts the possibledomains of V1 V2 and V3

An ICSP is a mathematical problem whose solution is theminimal domain of the variables satisfying all the constraints

26 Interval Constraint Propagation (ICP) To solve an ICSPan Interval Constraint Propagation (ICP) algorithm iteratesdomain reductions until no domain can be contracted Tosatisfy the set of 119901 constraints it removes from the domainof the variable every value that is not compatible with theconstraints and the other variables ICP reduces the size ofthe domains in a consistent way by repeating this removaloperation This solving process is called 119888119900119899119905119903119886119888119905119894119900119899 and thecorresponding algorithm (eg ForwardBackward) used forcontraction is regarded as a 119888119900119899119905119903119886119888119905119900119903

Let us consider a simple ICSP instance which is definedby 3 variables and 2 constraints119911 = 119909 + ln (119910)119910 = 1199112 (12)


The domains of 119909 119910 and 119911 are respectively [119909] [119910] and [119911]An inclusion function of (12) is[119911] = [119909] + ln[] ([119910])[119910] = [119911]2 (13)

The ForwardBackward Propagation (FBP) an ICP algo-rithm can be applied to solve the ICSP FBP executesthe contraction process in two phases firstly the Forwardpropagation reduces the left terms of (13) via[119911] = [119911] cap ([119909] + ln[] ([119910])) (14a)[119910] = [119910] cap [119911]2 (14b)

Secondly Backward propagation reduces the right termsof (13) by [119909] = [119909] cap ([119911] minus ln[] ([119910])) (15a)[119910] = [119910] cap exp[] ([119911] minus [119909]) (15b)[119911] = [119911] cap radic[] [119910] (15c)

Forward and Backward equations obtained from theconstraints are computed one after the other (here wecompute from (14a) to (15c)) When all the equations havebeen computed the algorithm restarts from the beginningequation until the domains of [119909] [119910] and [119911] are no morecontracted (or contracted less than a specified parameter)

3 Localization and Mapping

31 Problem Statement Let us consider a vehicle navigatingin a 2D environment At time step 119896 the vehicle pose isrepresented by a three-dimensional vectorX119896 = (119909119896 119910119896 120579119896)119879with 119909119896 119910119896 being the vehiclersquos position and 120579119896 the ori-entation The 119899 stationary landmarks are represented byM = (L1L2 sdot sdot sdot L119899)119879 L119894 is a state vector defining thelandmark position (see Section 33) For the sake of simplicitywe assume that the pose evolution follows 119872119886119903119896119900V 119888ℎ119886119894119899where the current pose X119896 depends only on the very lastpose X119896minus1 and the current control input vector U119896 Theinput vector U119896 consists of linear and angular elementarymovements which were collected by proprioceptive sensors(eg odometer and gyro) If these odometric measurementsare available the state vector X119896 can be evaluated throughthe dead reckoning function (also called motion model)expressed as follows

X119896 = 119891 (X119896minus1U119896) + 119908119896 (16)

where 119908119896 is the measurement noise affecting the odometricmeasurements

The vehicle is equipped with exteroceptive sensorsrecording information about the environment At each timestep 119896 the vehicle observes a number of landmarks which canbe regarded as a subset of the entire map the measurementZ119896 depends only on the current vehicle poseX119896 and themapstateM

Z119896 =H (X119896M) + 119903119896 (17)

where H(sdot) is generally a nonlinear function it differs fordifferent types of exteroceptive sensors The output ofH(sdot) isa geometric parameterization of the landmark (eg distanceangle or pixel coordinates) and 119903119896 is the measurement noiseof the exteroceptive sensors which introduce uncertainty tothe landmark state

Interval methods are based on the assumption that thesupport of sensor noises is unknown but bounded by realintervals Mathematically forall119896 gt 0 119908119896 isin [w] and 119903119896 isin [r]where [w] and [r] are real intervals defined by lower andupper boundsThen the localization problem can be regardedas an ICSP the set of variables is defined by the vehiclepose and the landmark position the motion and observa-tion functions ((16) and (17)) define the constraints ICPtechniques allow us to propagate the interval uncertaintiesin a consistent way without any probability hypothesis orlinearization process As a result all possible solutions arefound and guaranteed

32 Motion Model The motion estimation process uses themeasurements of proprioceptive sensors to predetermine thelocalization vector [X119896] EKF use a probabilistic motionmodel to fuse sensor data [30] We make an extension of thismodel to obtain an interval based model defined by Seignezet al [18][X119896] = 119891[] ([X119896minus1] [U119896]) (18)

([119909119896][119910119896][120579119896])=([119909119896minus1] + [120575119904119896] cos[] ([120579119896minus1] + [1205751205791198962 ])[119910119896minus1] + [120575119904119896] sin[] ([120579119896minus1] + [1205751205791198962 ])[120579119896minus1] + [120575120579119896] ) (19)

where [U119896] = ([120575119904119896] [120575120579119896])119879 is the input vector 120575119904119896 and120575120579119896 represent respectively the elementary displacement androtation

The elementary movements can be deduced from rawodometric data (odometer and gyro) using an interval basedstatic fusion method[120575119904119896] = [120587] ([120596119897] [120575119901119897] + [120596119903] [120575119901119903])[119875][120575120579119896] = [120587] ([120596119897] [120575119901119897] minus [120596119903] [120575119901119903])[119890] sdot [119875] cap [120575120579119892119910119903119900] (20)

where 120575119901119897 and 120575119901119903 are the odometer measurements of the leftand right wheels 120596119897 and 120596119903 represent the radius of the wheels119875 is the odometer resolution and 119890 is the length of the rearaxle 120575120579119892119910119903119900 is the gyromeasurement For further informationabout interval based odometric sensor integration the readermight refer to [31] Once we have constituted intervalsfrom sensor data the vehicle pose [X119896] can be estimatedby applying FBP algorithm (Section 26) over constraintsdeduced from the motion model


33 Landmark Parameterization A landmark (or featurepoint) is a 3D point in the global world Probabilisticmethodsgenerally represent landmark with an inverse depth parame-terization [32]Theymodel the uncertainty of each parameterby Gaussian distributions However it turns out to be not soefficient to represent the linear depth uncertainty of monocu-lar vision Interval analysis provides an easy and efficient wayto parameterize landmarks Each landmarkL119894 is defined as asix-dimensional state vector ([119909119900] [119910119900] [119911119900] [120572119894] [120593119894] [119889119894])119879which models the estimated landmark position at[L119894] = ([119909119900] [119910119900] [119911119900])119879 + [119889119894] sdot [m] ([120572119894] [120593119894]) (21)

where coordinates [119909119900] [119910119900] and [119911119900] represent the opticalcenter of the camera when the landmark was seen for the firsttime [120572119894] and [120593119894] represent azimuth and elevation angle forthe ray which traces the landmark and [119889119894] is the depth of thelandmark [m]([120572119894] [120593119894]) is a unitary vector pointing from thecamera to the landmark [L119894]

[m] ([120572119894] [120593119894]) = ( cos[] ([120572119894]) cos[] ([120593119894])minussin[] ([120572119894]) cos[] ([120593119894])sin[] ([120593119894]) ) (22)

Since all the parameters are represented by intervals and[119889119894] can be initialized as [0 +infin] the landmarkrsquos uncertaintyis modeled as an infinite cone which combines the vehiclepose uncertainty and the observation uncertainty It is arealistic representation for the monocular vision uncertaintyThe major advantage is that the initialization of [119889119894] isundelayed guaranteed and efficient for landmarks over awide range as it always includes all possible value without anyprior information

34 Observation Model The observation of a landmark isthe coordinates of the pixel where the landmark is projectedon the image In order to detect and extract landmarkinformation from an image we used Speed Up RobustFeatures (SURF) [33] The work [34] recommended it after acomparison between several algorithms showing that SURFprovide results with a very low error rate The projection ofthe 119894119905ℎ landmark to the image frame can be formulated asfollows

From (21) the landmarkrsquos position in the world coordi-nateO119908 can be expressed[L119908119894 ] = ([119909119900] [119910119900] [119911119900])119879 + [119889119894] sdot [m] ([120572119894] [120593119894]) (23)

In the camera coordinateO119888 the landmark [L119908119894 ]will be seenin the position [L119888119894 ][L119888119894 ] = [119877119908119888] ([L119908119894 ] minus [119877119903119908] [X119896]) (24)

where [X119896] is the current vehicle pose 119877119908119888 and 119877119903119908 arerespectively the transformational matrix between the worldand the camera coordinate and the robot and world coordi-nate respectively

The camera does not observe [L119888119894 ] directly but its pro-jection in the image frame (119906 V)119879 According to the pin-hole

camera model [35] the observationZ119894119896 of [L119888119894 ] on the imageframe can be predicted by[Z119894119896] =H[] ([X119896] [L119888119894 ]) (25)

([119906119894119901119903119890][V119894119901119903119890]) = ([119888119906] + [119891] [119896119906] [L119888119894 (119909)][L119888119894 (119911)][119888V] + [119891] [119896V] [L119888119894 (119910)][L119888119894 (119911)]) (26)

where 119891 119896119906 119896V 119888119906 119888V are the camera intrinsic parametersEquation (26) is also used as the projection function

35 Data Association Data association focuses on findingout the correspondence between the map and observationsDifferent from probabilistic method which deals with uncer-tainty ellipsoid in projection procedure interval methodproceeds with bounding boxThematching process is carriedout as follows

(i) Project the landmarks to the current image planediscard thosewhich lay out of the image bounds Eachlandmark defines a rectangle searching area

(ii) In the searching area search for the candidate featurethat is matched to the landmark We adopt Danrsquosmethod [36] which performs a two-stage matchingprocess (considering both the Euclidean distance anddominant orientation information of feature descrip-tors) to provide a robust and accuracy matchingresult For each pair of matched features a ZeroNormalized Cross Correlation (ZNCC) [37] score iscomputed in themapping stage to help to evaluate therobustness of the landmarks

(iii) Filtering step is necessary to keep a desirable amountof matched points The landmark uncertainty andZNCC score can be used for the selection processWe make the matched results distributed uniformlyover the image providing enough parallax for thelocalization

36 ICSP Formulation The data association process gener-ates a set of 3D to 2D correspondences which we can use tobuild the ICSP constraints Let us consider the 119894119905ℎ landmark[L119894] that has been matched The observation of [L119894] is([119906119894119900119887119904] [V119894119900119887119904])119879 The predicted position on the image planedenoted by ([119906119894119901119903119890] [V119894119901119903119890])119879 is computed via (23) (24) and(26) The observation can be used to correct the system statethrough the predicted observation[119906119894119901119903119890] = [119906119894119900119887119904][V119894119901119903119890] = [V119894119900119887119904] (27)

Equation (27) creates a link between the prediction andobservation data of the 119894119905ℎ landmark [L119894] This link (thanksto (23) (24) (26) and (27)) imposes constraints on [X119896] and[L119894] which can be used to update both the vehicle pose andlandmark position


OdoGyro

Camera

StaticFusion

DisplacementModel

BuildICSPs

DataAssociation

Map

ImageProcess

Projection NewFeature

Solver

Update

Motion

Observation

uk Xk

Xlowastk

Xlowastkminus1

Figure 4 Mapping framework

Based on the above derivation an ICSP composed of 2top level constraints and a set of 9 variables can be set up Foreach matched landmark 119894 the ICSP119896(119881119896 119863119896 119862119896) is generatedand defined as follows

(i) A set of 9 variables119881119896 = 119909119896 119910119896 120579119896 119909119900 119910119900 119911119900 120572119894 120593119894 119889119894 (28)

(ii) A set of 9 domains119863119896 = [119909119896] [119910119896] [120579119896] [119909119900] [119910119900] [119911119900] [120572119894] [120593119894] [119889119894] (29)

(iii) A set of 2 top level constraints119862119896 = (1198621119896 1198622119896)119879 (30)1198621119896 lArrrArr [119906119894119901119903119890] = [119906119894119900119887119904]1198622119896 lArrrArr [V119894119901119903119890] = [V119894119900119887119904] (31)

If the data association process found 119873 matched land-marks then we get a multiple ICSP to solve

]ICSP119896 = (ICSP1119896 ICSP2119896 sdot sdot sdot ICSP119873119896 ) (32)

Each ICSP contains constraints between the vehicle pose anda unique landmark position The good point is that they areall linked together via the same variables 119909119896 119910119896 and 120579119896 (thevehicle pose at current time 119896) This correlation makes ICPalgorithms very powerful to jointly contract the domains ofall variables

37 System Overview Figure 4 depicts the framework of themapping stage It is an interval SLAM process which is castinto two parts motion and observation In the motion partthe preliminary pose of the vehicle is predicted by fusing themeasurements of the odometers and gyrowith bounded errormodel in the observation part features are firstly extractedfrom the image data association task aims at finding outthe correspondences between the features and the map newfeatures will be initialized as new landmarks and added to themap and old ones will be processed to generate new ICSPsthen these ICSPs are jointly solved by an ICP algorithmwhich correct the vehicle pose and update the map

(1) while Localization is required do(2) Input [X119896minus1] [U119896] [M] 119868119896(3) Output [X119896](4) [X119896] larr 119891[]([X119896minus1] [U119896])(5) 119878119896 larr ExtractFeature(119868119896)(6) [(119906 V)119900119887119904L]119873 larr DataAssociate([X119896] [M] 119878119896)(7) for 119895 larr 1 to119873 do(8) ICSP119895 larr Build([X119896] [(119906 V)119900119887119904L]119895)(9) end for(10) ]ICSP119896 = ⋃119895=1 to 119873

ICSP119895(11) [Xlowast119896 ] larr Solver(]ICSP119896)(12) end while

Algorithm 1 Localization

The localization stage is quite similar to the mappingstage There are two main differences high cost sensors areno longer used and no new feature is added to the mapAlgorithm 1 gives the pseudocode of our localizationmethod

(i) Line (4) motion estimation The current vehicle poseis calculated based on the motion function 119891[] andinput vector [U119896]

(ii) Line (5) image processing A set of SURF 119878119896 areextracted from the new image 119868119896

(iii) Line (6) data association The correspondencebetween the features and the map is found

(iv) Line (8) ICSP generating New ICSP are generatedbased on the matched landmarks

(v) Line (10)-(11) Solving ]ICSP119896 is composed by theICSPs and solved by an ICP algorithm

4 Simulation Results

A simulation experiment is set up to evaluate the perfor-mance of our proposed ICSP based mapping and localizationmethod The vehicle starts from the origin (see Figure 5) andmoves straight forward in the positive direction of x-axis withconstant linear and angular velocity (120592 = 01119898 sdot 119904minus1 120596 = 0)5 landmarks are positioned in the surrounding environmentwithout any prior knowledge of their positions At each time


0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle

(a) Without vehicle pose uncertainty

minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)

(b) With vehicle orientation uncertainty

Figure 5 Interval based landmark initialization

step the vehicle detects the 5 landmarks with a simulatedcamera model [119891 119896119906 119896V 119888119906 119888V] = [40 8 8 320 240]) andfocuses on estimating their position The experiment is firstcarried out without odometric sensor noise (the vehicle poseis perfectly known) to verify the feasibility of our methodThis condition (no odometric noise) is then released in theexperiment presented in Section 43

41 Landmark Initialization At time step 119896 = 1 the 5landmarks are first detected and the initialization procedureis executed Figure 5(a) shows the top view of the initializa-tion result of landmarks (see Section 33) Each landmarkrsquosuncertainty is initialized as an infinite cone because of thelacking information about the landmark depth As thereis no uncertainty on vehicle pose the landmarks are wellinitialized and the opening angles which indicate the obser-vation uncertainty are small (due to the bounded observationnoise which is 1 pixel) If the vehicle has an uncertainty onits orientation (the heading angle is not perfectly defined[120579119896] = [minus120587180 120587180]) then the cones of uncertaintyare much larger than the previous ones (see Figure 5(b))This is reasonable since the cone uncertainty is the fusionof observation uncertainty and vehicle pose uncertaintyBecause our bounded error hypothesis and the use of intervalanalysis the initialization result is guaranteed to contain thetrue landmark position

42 Landmark Uncertainty Evolution As the vehicle movesthe landmarks are repeatedly observed with different par-allaxes By building and solving the associated ICSP (seeSection 36) each of the landmarkrsquos parameters can be esti-mated Figure 7 shows since initialization the evolution ofthe poses uncertainties of landmarks 1 2 and 5 in top viewEach row plots the estimation result at time steps 1 5 10 and

Camera poseTime step k=1 k=n-1 k=n

Figure 6 Parallax observations

20 The parallax 120573 (defined as the angle difference betweenthe initial observation and current observation see Figure 6)is displayed as the title of each subfigureWith the estimationproceeds the landmark uncertainty estimate improves As wecan see the bigger the parallax is the better the contractionof the landmark uncertainty will be When enough parallaxis eventually available the infinite interval uncertainty issignificantly reduced The uncertainty of landmark 3 keepsinfinite because it locates on axis 119909 and no parallax has beendetected during the horizontal translation

To visualize the evolution of a landmark state it is neces-sary to convert the 6-dimensional representation to a global3D point using (21) (119909119900 119910119900 119911119900 120572119894 120593119894 119889119894)119879 rarr (119909119894 119910119894 119911119894)119879 Theinterval uncertainty (IU) can be evaluated by computing thebox volume [31] 119880(119894) = 119908([119909119894]) times 119908([119910119894]) times 119908([119911119894]) As thedepth of each landmark is initialized as [0 +infin] the 119909 119910and 119911 coordinates have infinite value and the uncertaintyturns out to be infinite at the first hand After multipleobservations (at different times) fromdifferent parallaxes thelandmarkrsquos state is updated Figure 8 depicts the evolutionof the uncertainty associated with the landmarks 1 2 4and 5 The 4 landmarks encounter different observationparallaxes so the contraction of their uncertainties is notsimultaneous However they exhibit the same performance


0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5

Step1 Step5 Step10 Step20

= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘

Figure 7 Interval uncertainty evolution

0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number

Landmark 1Landmark 2


Unc

erta

inty

(Ｇ3)

Figure 8 Landmark position uncertainty evolution

the interval uncertainty of each landmark is well propagatedand decreases exponentially with the vehicle navigating in theenvironment

43 Estimation Accuracy In real world the vehicle pose isalways accompanied with uncertainties due to the cumulativeodometric noise The vehicle pose uncertainty will affectthe estimation accuracy of the landmarks (a smaller vehiclepose uncertainty means higher landmarks estimation accu-racy) In our simulation the error bound of the odometricmeasurement is supposed to be proportional to elementarydisplacement such that 119908([U119896]) le 120574 sdot U119896 + Γ Γ =[00001119898 001∘]119879 with U119896 being the real odometric mea-surement 120574 denoting the ratio value (bigger 120574 value indicatesa relative bigger uncertainty of odometric measurements)and Γ accounting for the steady system error The averageposition uncertainty of the 4 landmarks has been computedat each time step Figure 9 displays the results with different120574 value (120574 = 001 01 and 02) As it is shown the estimationaccuracy decreases when 120574 value becomes bigger This resultsupports the fact that the landmark estimation accuracy isstrongly correlated with the vehicle pose uncertainty Bigger


5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020

Figure 9 Average landmark uncertainty under different 120574 valuesodometric uncertainty will result in a worse estimation ofvehicle pose as a result the landmark estimation accuracydecreases

5 Real Data-Set Experiment

51 Experiment Set-Up To evaluate the performance of ourmethod in realistic circumstance we used the experimentaldata set from Institute Pascal [38] It contains multisensorytimestamped data which can be used in a large variety ofapplications It includes data from low-cost sensors groundtruth and multiple sequences as well as data for calibra-tion Data were collected on the VIPALAB platform (seeFigure 10(a)) driving on the PAVIN track (see Figure 11(a))an experimental site located on the French campus of BlaisePascal University The environment is composed of scaledstreet road markings functional traffic lights painted wallsbuildings vegetation and so on (see Figure 11(b)) The plat-form was equipped with multiple sensors (see Figure 10(b))In our experiment we need to use 4 sets of timestampedsensor data

(i) Odometers each rear wheel is equipped with anodometer in the formof a ring gear providing 64 ldquotoprdquoper wheel revolution

(ii) Gyro (Melexis MLX90609-N2) it measures the rota-tional speed in an inertial reference system Vibratingsilicon structures that use the Coriolis force outputthe yaw rate from which we can deduce the elemen-tary rotation

(iii) Camera (FOculus FO432B camera + Pentax C418DXlens) it provides information about the perceivedenvironment We use the camera which located in theleft-front of the vehicle and looking ahead

Table 1 Average bounding boxes size

Parameter 119909 119910 119911Mean width (m) 0207 0221 0013

(iv) RTK-GPS the data acquired with an embeddedProFlex500 RTK-GPS receiver from NavtechGPScoupled with a Sagitta Magellan GPS base station areconsidered as the ground truth This system providesan accurate absolute localization measurement towithin 2 cm (plusmn1 cm)

52 Mapping Stage A mapping stage is first conducted byour method in order to build a map of the environment Thevehicle follows 6 simple consecutive loops around the PAVINenvironment Figure 12(a) shows the reference trajectorythat the vehicle passed by and Figure 12(b) details the 6loops in order As we have discussed in Section 43 theestimation of landmark is strongly correlated with the vehiclepose uncertainty So it is necessary to maintain the poseuncertainty in a reasonable size during the mapping stage inorder to pursue a precise map

Figure 13 shows the vehicle position uncertainty (calcu-lated by (119908([119909])2) sdot (119908([119910])2)) with and without RTK-GPScorrection during the mapping process As we can see with-out correction the position uncertainty is cumulative andincreases very quickly until loop closure is realized This isbecause when navigating the odometric noise is cumulativesince there is no absolute measurement Interval analysis isa pessimistic method maintaining all possible solutions thecumulative errors will result in interval expansion and thepose uncertainty thus becomes larger and larger This is ahinder to pursue a good map To overcome this problemour method makes use of the RTK-GPS measurement andthe GPS error bounds are characterized as the maximumimprecision (le 2119888119898) of the GPS receiver The cumulativeerrors can be eliminated and a precise estimation of thevehicle position is obtained (see Figure 13(b)) The result isassumed to be consistent since only ICP technique was used

At the end of the loop 3450 images are processed anda map of 1140 landmarks is created Figure 14 shows partof the map projected on 119909 minus 119910 plane The black boundingboxes denote the estimated landmarkrsquos position The averagesize of these bounding boxes is shown in Table 1 which canbe regarded as a factor to evaluate the quality of the mapThe 119911 coordinate gets much higher precision than 119909 and 119910this is because the estimation of 119911 is only correlated withthe observation elevation angle which is invariant wrt thevehiclersquos heading angle while 119909 and 119910 are affected by theheading uncertainty A good way to improve the map isto get a better estimation of the vehiclersquos orientation Notethat the map building process could be done offline suchthat computation intensive ICP algorithms and manuallyguidance could be used to obtain a better map

53 Localization Result The localization stage benefits fromthe map data which offer position constraints With thebuilt map the vehicle seeks consistent localization using the


(a) VIPALAB platform (b) Vehicle structure

Figure 10 VIPALAB Experimental platform

(a) The PAVIN track (b) Few images of the environment

Figure 11 Institute Pascal Experimental Environment

current image captured by the monocular camera Threesequences of data from the whole data set are used to test ourlocalization method The reference path (RTK-GPS) of thevehicle when these data are collected is illustrated in Figures15 18(a) and 18(b)

Our proposed localization method is firstly tested onSequence 1 and the vehicle follows a nearly 200m trajectorysee Figure 15 The blue path is the reference trajectory fromRTK-GPS the red path is the dead reckoning result andthe black squares are the localization boxes output by ourproposed interval method The dead reckoning result isobtained via a geometric evolution model using the odometerand steering angle data It can be seen from the figurethat the localization boxes followed the reference trajectoryin a consistent way (intersect with the blue line) Table 2(DR dead reckoning IM interval method) presents thelocalization result of both methods at 119905 = 200 and 119905 = 400and at the end of the track By fusing the map our interval

method gives a consistent estimation of the vehicle pose thelocalization boxes well enclose the ground truth

To compare the vehicle position estimated by our pro-posedmethod and the dead reckoning the root of sum squareerror (RSSE) is computed at each time step It is expressed bythe following formula

RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)

where 119864119896(119909) and 119864119896(119910) are the estimation error of mid([119909])position and mid([119910]) position at time step 119896 RTK-GPS datais regarded as the ground truth

Figure 16 shows the RSSE results of both methods As itcould be expected the cumulative odometric error of deadreckoning is significant and caused an increasing discrepancy(at the end the discrepancy reaches about 3 meters) Ourmethod takes advantage of the map data and corrects theodometric error providing a very small discrepancy


minus30minus40minus50 minus20 minus10 0 10minus20minus15minus10

minus505

1015202530

x (m)

y (m

)

(a) Full mapping path

minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3

(b) Consecutive single loop

Figure 12 The reference path (RTK-GPS) of mapping sequence

0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)

(a) Without RTK-GPS correction

0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4

(b) With RTK-GPS correction

Figure 13 Vehicle position uncertainty

In order to verify the consistency of the localizationresults interval error is defined by the upper and lowerbounds of the estimated state minus the correspondingreference (real value) mathematically[119864119896 (120572)] = [120572119896 minus 120572lowast119896 120572119896 minus 120572lowast119896 ] (34)

where [120572119896 120572119896] is the estimated state of variable 120572 at time step119896 and 120572lowast119896 is the corresponding reference The estimated stateis said to be consistent if and only if 0 isin [119864119896(120572)] is a truestatement Moreover an estimation method is consistent and

precise if forall119896 isin 1 2 sdot sdot sdot infin 0 isin [119864119896(120572)] is always satisfiedand the interval error width 119908([119864119896(120572)]) is thin

We compute the real-time interval error of our localiza-tion result At each time instant 119896 when the new capturedimage had been processed 119864119896(119909) and 119864119896(119910) are calculatedrespectively Figure 17 depicts the interval corridors consist-ing of the upper and lower bounds of 119864119896 The zero line iswell included by those corridors proving that the localizationresults are consistent all along the track This is a significantresult in vehicle localization field where safety is a crucialissue


minus43 minus42 minus41 minus40 minus39 minus38 minus37 minus36

minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path

Figure 14 Map building result

minus50 minus40 minus30 minus20 minus10 0 10minus20

minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START

Dead reckoning Reference path

Localization box

x (m)

y (m

)

Figure 15 Localization trajectory of Sequence 1

Table 2 Localization result

Time Position RTK DR IM

t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]

Similar results are obtained when performing the local-ization process on Sequences 2 and 3 with the same mapthe output localization boxes by our method and the cor-responding reference trajectory are displayed in Figure 18It shows the robustness of our method providing consistent

RSSE

(m)

35

3

2

25

15

1

05

0

Dead reckoningInterval method

step number0 100 200 300 400 500 600

Figure 16 The error of localization results by DR and IA

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y

Figure 17 Interval error of real-time localization result

localization result The width of the interval error corridors(see Figure 17) with respect to the vehiclersquos 119909 position and 119910position is displayed in Figure 19 reporting amaximumvalueof 06 m The average interval error width and mean RSSEare computed to evaluate the localization accuracy Table 3details the results of the 3 experiments Computing an averagevalue for the 3 experiments gives 127 cm accuracy for 119909 and



minus15

minus10

minus5

0

5

10

15

20

25

30

Reference pathLocalization box

x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3

Figure 18 Localization boxes compare with reference path

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)

Sequence 1Sequence 2Sequence 3

(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)

Figure 19 Interval error width of localization result

144 cm for 119910 The average RSSE value is about 966 cm (lessthan 10 cm)Note that these localization results are consistentand the localization boxes are guaranteed to enclose the realpositions of the vehicle

54 Discussion In this paper we present a complete method-ology for localizing a vehicle within a prebuilt map We bringforward a two-stage framework (mapping and localization)to solve the problem Closer approaches have already beenproposed by different researchers [39ndash41] They all involve a

Table 3 Mean localization interval error and RSSE

Experiment x (cm) y (cm) RSSE (cm)Sequence 1 117 134 890Sequence 2 103 127 829Sequence 3 161 172 118

visual learning step to reconstruct a map of the environmentand they use this map for localization and navigation task


Eric Royer et al [39] uses bundle adjustment inmapping stepand the localization results are obtained via Newton iterationmethod Hyon Lim et al [41] uses a structure from motion(SFM) algorithm to reconstruct the 3D environment then inthe localization step two discrete Kalman filters are employedto estimate the camera trajectory Courbonrsquos method [40]does not involve the 3D reconstruction procedure during themap building step instead it records some key views andthe performed path as references and uses them for futurenavigation missions Ourmethod uses totally different theoryand techniques from these approaches we cast the two-stageproblem into an ICSP and deterministic techniques (ICP) areused in order to find the solution

The localization accuracy of these works is close to oursThe paper [39] has an average localization error of 15 cmover 6 localization experiments on 3 outdoor sequences Thereported mean indoor localization errors of [41] are rangedbetween 56 cm and 108 cm among 6 experiments Theoutdoor mean position error is less than 15 cm The paper[40] presents an average lateral error of 25 cm on an urbanvehicle navigating along a 750 m trajectory Our methodhas been tested on 3 outdoor sequences the position error(RSSE) ranges between 829 cm and 118cm The strength ofour method is that the localization boxes are guaranteed tocontain the real position of the vehicle Such a guaranteeis due to the modeling of the noises as intervals and thecomputing of constraints using interval functions

The formulas of our algorithm work whatever the cameramanufacturer is As for different cameras the observationmodel always holds only the camera intrinsic parame-ters (119891 119896119906119896V 119888119906 119888V) change Furthermore the feature pointsextracted from images of various cameras would be the samesince the SURF algorithm is scale invariant rotation invari-ant and affine transformation invariant [33] This propertymakes it possible to find correct correspondence betweentwo images from different cameras This feasibility has beenshown by [39] in two sequences of images taken by a pairof cameras one sequence was used in the map learning stepand the other one was used for vehicle localization the resultturns out to be robust

6 Conclusion

In order to overcome the inconsistency problem of prob-abilistic methods this paper proposed a consistent vehiclelocalization method based on interval analysis We proposea 2-stage process mapping and localization The map isbuilt by a leader vehicle equipped with a sensor or a set ofsensors providing an accurate positioning The built map canbe beneficial for a fleet of vehicles intending to achieve aconsistent localization in the environment By fusing low-cost dead reckoning sensors and map data the proposedlocalization method is able to locate the vehicle consistentlyin the environment using the current image taken by thecamera The cumulative odometric error is eliminated andthe localization consistency is maintained Our method isvalidated with an outdoor car like vehicle equipped withodometers gyro and monocular camera The experimentshighly illustrate the consistency of the localization results

provided by our method Future work will deal with a formalproof of the obtained consistency

Our methodology can be used to solve various problemseven problems with higher DOF Indeed the used ICPalgorithms express a polynomial time with respect to thenumber of variables in the ICSP [42] so our ICP basedmethod is capable of scaling up to higher DOF cases withouthigh CPU load In order to adapt to other applications wehave only to define a new motion model

Data Availability

Data used in this manuscript are openly available atldquohttpipdsuniv-bpclermontfrrdquo see [38] for more details

Conflicts of Interest

The authors declare that they have no conflicts of interest

References

[1] M Adams S Zhang and L Xie ldquoParticle filter based outdoorrobot localization using natural features extracted from laserscannersrdquo in Proceedings of the Proceedings- 2004 IEEE Interna-tional Conference on Robotics and Automation pp 1493ndash1498May 2004

[2] A Gning and P Bonnifait ldquoGuaranteed dynamic localizationusing constraints propagation techniques on real intervalsrdquoin Proceedings of the Proceedings- 2004 IEEE InternationalConference on Robotics and Automation pp 1499ndash1504 May2004

[3] D Gruyer R Belaroussi and M Revilloud ldquoMap-aided local-ization with lateral perceptionrdquo in Proceedings of the 25th IEEEIntelligent Vehicles Symposium IV 2014 pp 674ndash680 June 2014

[4] A Howard M Matark and G Sukhatme ldquoLocalization formobile robot teams using maximum likelihood estimationrdquo inProceedings of the IROS2002 IEEERSJ International Conferenceon Intelligent Robots and Systems vol 1 pp 434ndash439 LausanneSwitzerland 2002

[5] D Gruyer A Lambert M Perrollaz and D Gingras ldquoExper-imental comparison of bayesian positioning methods basedon multi-sensor data fusionrdquo International Journal of VehicleAutonomous Systems vol 12 no 1 pp 24ndash43 2014

[6] S Thrun W Burgard and D Fox ldquoA Probabilistic Approachto Concurrent Mapping and Localization for Mobile RobotsrdquoAutonomous Robots vol 5 no 3-4 pp 253ndash271 1998

[7] W Burgard D Fox D Hennig and T Schmidt ldquoEstimating theabsolute position of a mobile robot using position probabilitygridsrdquo in Proceedings of the 1996 13th National Conference onArtificial Intelligence pp 896ndash901 August 1996

[8] F Dellaert D Fox W Burgard and S Thrun ldquoRobust MonteCarlo localization for mobile robotsrdquo Artificial Intelligence vol128 no 1-2 pp 99ndash141 2001

[9] S Huang and G Dissanayake ldquoA critique of current develop-ments in simultaneous localization andmappingrdquo InternationalJournal of AdvancedRobotic Systems vol 13 no 5 pp 1ndash13 2016

[10] MDiMarco A Garulli A Giannitrapani andA Vicino ldquoA settheoretic approach to dynamic robot localization andmappingrdquoAutonomous Robots vol 16 no 1 pp 23ndash47 2004

[11] R E Moore Interval Analysis Prentice-Hall Englewood CliffsNJ USA 1966


[12] A Gning and P Bonnifait ldquoConstraints propagation techniqueson real intervals for guaranteed fusion of redundant dataapplication to the localization of car-like vehiclesrdquoAutomaticavol 42 no 7 pp 1167ndash1175 2006

[13] L JaulinApplied Interval Analysis with Examples in ParameterAnd State Estimation Robust Control And Robotics Springer2001

[14] T Rassi N Ramdani and Y Candau ldquoSet membership stateand parameter estimation for systems described by nonlineardifferential equationsrdquoAutomatica vol 40 no 10 pp 1771ndash1777(2005) 2004

[15] M Kieffer L Jaulin E Walter and D Meizel ldquoRobustautonomous robot localization using interval analysisrdquo Reli-able Computing An International Journal Devoted to ReliableMathematical Computations Based on Finite Representationsand Guaranteed Accuracy vol 6 no 3 pp 337ndash362 2000

[16] L Jaulin M Kieffer E Walter and D Meizel ldquoGuaranteedrobust nonlinear estimation with application to robot localiza-tionrdquo IEEE Transactions on Systems Man and Cybernetics PartC (Applications and Reviews) vol 32 no 4 pp 374ndash381 2002

[17] M Kieffer E Seignez A Lambert E Walter and T MaurinldquoGuaranteed robust nonlinear state estimator with applicationto global vehicle trackingrdquo in Proceedings of the 44th IEEEConference on Decision and Control and the European ControlConference CDC-ECC rsquo05 pp 6424ndash6429 esp December 2005

[18] E Seignez M Kieffer A Lambert E Walter and T MaurinldquoReal-time bounded-error state estimation for vehicle trackingrdquoInternational Journal of Robotics Research vol 28 no 1 pp 34ndash48 2009

[19] A Lambert D Gruyer B Vincke and E Seignez ldquoConsistentoutdoor vehicle localization by bounded-error state estimationrdquoin Proceedings of the 2009 IEEERSJ International Conferenceon Intelligent Robots and Systems IROS 2009 pp 1211ndash1216October 2009

[20] I K Kueviakoe A Lambert and P Tarroux ldquoA real-time inter-val constraint propagation method for vehicle localizationrdquoin Proceedings of the 2013 16th International IEEE Conferenceon Intelligent Transportation Systems Intelligent TransportationSystems for All Modes ITSC 2013 pp 1707ndash1712 nld October2013

[21] V Drevelle and P Bonnifait ldquoRobust positioning using relaxedconstraint-propagationrdquo in Proceedings of the 23rd IEEERSJ2010 International Conference on Intelligent Robots and SystemsIROS 2010 pp 4843ndash4848 twn October 2010

[22] K Lassoued P Bonnifait and I Fantoni ldquoCooperative Local-ization with Reliable Confidence Domains Between VehiclesSharingGNSSPseudoranges ErrorswithNoBase Stationrdquo IEEEIntelligent Transportation SystemsMagazine vol 9 no 1 pp 22ndash34 2017

[23] B Desrochers and L Jaulin ldquoA minimal contractor for thepolar equation Application to robot localizationrdquo EngineeringApplications of Artificial Intelligence vol 55 pp 83ndash92 2016

[24] L Jaulin A Stancu and B Desrochers ldquoInner and outerapproximations of probabilistic setsrdquo in Proceedings of theAmerican Society of Civil Engineers (ASCE) Second InternationalConference on Vulnerability and Risk Analysis and Managementand Sixth International Symposium on Uncertainty Modellingand Analysis pp 13ndash16

[25] L Jaulin ldquoRange-only SLAM with occupancy maps A set-membership approachrdquo IEEE Transactions on Robotics vol 27no 5 pp 1004ndash1010 2011

[26] P T I K Kueviakoe andA Lambert ldquoVehicle localization basedon interval constraint propagation algorithmsrdquo in Proceedingsof the IEEE International Conference on Cyber Technology inAutomation Control and Intelligent Systems pp 179ndash184 May2013

[27] R E Moore R B Kearfott and M J Cloud Introduction toInterval Analysis Society for Industrial Mathematics 2009

[28] F Le Bars E Antonio J Cervantes C De La Cruz and L JaulinldquoEstimating the trajectory of low-cost autonomous robots usinginterval analysis Application to the eurathlon competitionrdquo inMarine Robotics and Applications pp 51ndash68 Springer 2018

[29] E Hyvonen ldquoConstraint reasoning based on interval arith-metic the tolerance propagation approachrdquo Artificial Intelli-gence vol 58 no 1-3 pp 71ndash112 1992

[30] A Lambert and N Le Fort-Piat ldquoSafe task planning integratinguncertainties and local maps federationsrdquo International Journalof Robotics Research vol 19 no 6 pp 597ndash611 2000

[31] B Vincke A Lambert D Gruyer A Elouardi and E SeignezldquoStatic and dynamic fusion for outdoor vehicle localizationrdquoin Proceedings of the 11th International Conference on ControlAutomation Robotics and Vision ICARCV 2010 pp 437ndash442December 2010

[32] J M M Montiel J Civera and A J Davison ldquoUnified inversedepth parametrization for monocular SLAMrdquo in Proceedingsof the 2nd International Conference on Robotics Science andSystems (RSS rsquo06) pp 81ndash88 August 2006

[33] H Bay A Ess T Tuytelaars and L Van Gool ldquoSpeeded-uprobust features (surf)rdquoComputer Vision and Image Understand-ing vol 110 no 3 pp 346ndash359 2008

[34] A Schmidt M Kraft and A Kasinski ldquoAn Evaluation of ImageFeature Detectors and Descriptors for Robot Navigationrdquo inComputer Vision and Graphics pp 251ndash259 2010

[35] R Hartley and A Zisserman Multiple View Geometry inComputer Vision Cambridge University Press 2nd edition2003

[36] Z Dan and H Dong ldquoA robust object tracking algorithm basedon surfrdquo in Proceedings of the IEEE International Conference onWireless Communications amp Signal Processing (WCSP) pp 1ndash52013

[37] J P Lewis ldquoFast normalized cross-correlationrdquoVision Interfacevol 10 pp 120ndash123 1995

[38] H Korrapati J Courbon S Alizon and F Marmoiton ldquoTheinstitut pascal data sets un jeu de donnees en exterieur mul-ticapteurs et datees avec realite terrain donnees drsquoetalonnageet outils logicielsrdquo in Orasis Congres des jeunes chercheurs envision par ordinateur 2013

[39] E Royer M LhuillierM Dhome andT Chateau ldquoLocalizationin urban environments Monocular vision compared to a dif-ferential GPS sensorrdquo in Proceedings of the 2005 IEEE ComputerSociety Conference on Computer Vision and Pattern RecognitionCVPR 2005 vol 2 pp 114ndash121 June 2005

[40] J Courbon Y Mezouar and P Martinet ldquoAutonomous naviga-tion of vehicles from a visual memory using a generic cameramodelrdquo IEEETransactions on Intelligent Transportation Systemsvol 10 no 3 pp 392ndash402 2009

[41] H Lim S N Sinha M F Cohen M Uyttendaele and H JKim ldquoReal-time monocular image-based 6-DoF localizationrdquoInternational Journal of Robotics Research vol 34 no 4-5 pp476ndash492 2015

[42] G Chabert and L Jaulin ldquoContractor programmingrdquo ArtificialIntelligence vol 173 no 11 pp 1079ndash1100 2009

International Journal of

AerospaceEngineeringHindawiwwwhindawicom Volume 2018

RoboticsJournal of

Hindawiwwwhindawicom Volume 2018


Active and Passive Electronic Components

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics

Hindawiwwwhindawicom

Volume 2018

Hindawi Publishing Corporation httpwwwhindawicom Volume 2013Hindawiwwwhindawicom

The Scientific World Journal

Volume 2018

Control Scienceand Engineering

Journal of



Journal ofEngineeringVolume 2018

SensorsJournal of



RotatingMachinery


Modelling ampSimulationin EngineeringHindawiwwwhindawicom Volume 2018


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation


Hindawi

wwwhindawicom Volume 2018

Advances in

Multimedia

Submit your manuscripts atwwwhindawicom


area Instead of considering Gaussian noise all model andmeasurement noises are assumed to be bounded The aimis to guarantee the configurations of the robot pose that areconsistent with the given measurements and noise boundsKieffer [15] and Jaulin [16] provided simulation results for arobot equipped with a belt of on-board ultrasonic sensorsThe feasibility of interval methods on localization appli-cations had been shown Followed-up researchers focusedon real-time implementation of such localization techniqueKieffer [17] and Seignez [18] presented results for a mobilerobot navigating in an indoor environment with odometersmounted on each rear wheel to measure the motion andsonars located in different directions to perceive the environ-ment The ability of interval method to cope with erroneousdata and to obtain accurate estimations of the robot posewas demonstrated Lambert [19] extended such work byproviding results for an outdoor vehicle equipped withodometers gyro and GPS receiver Comparison was madewith the Particle Filter localization showing the better per-formance of interval method in terms of consistency Gning[2] and Kueviakoe [20] dealt with the outdoor localizationproblem in the framework of Interval Constraint SatisfactionProblem (ICSP) Those works used constraint propagationtechniques to fuse the redundant data of sensors Drevelle[21] uses relaxed constraint propagation approach to dealwith erroneous GPSmeasurements Bonnifait [22] combinedconstraint propagation and set inversion techniques andpresented a cooperative localization method with significantenhancement in terms of accuracy and confidence domainsExperimental results illustrate that constraint propagationtechniques are well adapted to real-time implementationand provide consistent result for localization Some otherresearchers proposed specific contractors [23] or separators[24] to solve the localization problem

Indeed when considering consistency issue intervalmethods are advantageous in vehicle localization Howeverthey are prone to large output imprecision which can makethem unsuitable for high level task We improve on itand present an extension of othersrsquo works dealing with thelocalization problem by using ICP Instead of using ultrasonicsensors [16] sonar [18 25] or GPS [19 26] as exteroceptivesensors we propose to use a monocular camera The aim isto achieve both consistent and low imprecision localizationresult We propose to drive a vehicle embedding precisebut expensive sensors around the environment in orderto build up a map represented in an interval way Thenthe proposed method enables a set or a single vehicle tonavigate consistently in such environment using this mapand low-cost sensors By constructing the problem as anICSP the proposed method is able to handle both mappingand localization in a unified form using Interval ConstraintPropagation techniques

The paper is organized as follows Section 2 introducesthe basics of interval analysis Section 3 illustrates how themapping and localization problem can be formulated as anInterval Constraint Satisfaction Problem Sections 4 and 5present the simulation and experimental results in terms ofmapping and localization Section 6 draws a conclusion of thepaper

2 Overview of Interval Analysis andConstraint Propagation

Interval analysis and constraint propagation are the the-oretical tools used in interval based methods for roboticlocalization and mapping In this section we give a briefoverview of these tools

21 Interval Analysis Interval analysis [11 27] is a numericalmethod which allows us to solve nonlinear problems in aguaranteed way One of the pioneers of interval analysisRamon E Moore proposed to represent a solution of a prob-lem by an interval in which the real solution is guaranteedAn interval [119909] is a connected subset ofR defined by its lowerbound 119909 and upper bound 119909[119909] = [119909 119909] = 119909 isin R | 119909 le 119909 le 119909 (1)

The width of a nonempty interval [119909] is calculated by119908([119909]) = 119909 minus 119909 and the midpoint (or center) is 119898119894119889([119909]) =(119909 minus 119909)2 If [119909] is an interval containing the 119909 position ofa vehicle 119898119894119889([119909]) can be taken as an estimation of 119909 and119908([119909])2 can be regarded as the estimation error [28]

Let IR be the set of the intervals of R A 119887119900119909 (alsocalled 119894119899119905119890119903V119886119897 V119890119888119905119900119903) [x] ([x] isin IR119899) is a generalizationof the interval concept It is a vector whose components areintervals [x] = [1199091] times [1199092] times sdot sdot sdot times [119909119899] (2)

For instance the configuration of a vehiclersquos pose usuallycontains 3 parameters position 119909 position 119910 and headingangle 120579 Consequently for vehicle localization the solution isa three-dimensional box [119909] times [119910] times [120579]

The width of a nonempty box [x] can be computed by119908([x]) = max119894=1119899119908([119909119894]) We define the volume of [x] asV119900119897 ([x]) = prod

1le119894le119899

119908 ([119909119894]) (3)

The volume of the box is usually used to evaluate theestimation uncertainty of a state vector [28]

22 Operations of Interval Arithmetic Rules have beendefined to apply the basic arithmetical operations on intervals[27] Let us consider two intervals [119909] and [119910] and a binaryoperator isin + minus times divide the smallest interval which containsall feasible values for [119909] [119910] is defined as follows[119909] [119910] = 119909 119910 | 119909 isin [119909] 119910 isin [119910] (4)

Example 1 Addition [minus4 3] + [2 5] = [minus2 8] subtraction[3 5] minus [1 2] = [1 4] multiplication [minus1 8] times [minus3 1] =[minus24 8] division [minus2 8] divide [2 4] = [minus1 4]The set-theoretic operations can be applied to intervals

The 119894119899119905119890119903119904119890119888119905119894119900119899 of two intervals is defined by[119909] cap [119910] = 119911 isin R | 119911 isin [119909] and 119911 isin [119910] (5)


[x] [x]

[x] cap [y] [x] ⊔ [y]

[y] [y]


[x]

f([x])

f[]([x])

IRm IRn





119898 119891 ([x]) sub 119891[] ([x]) (8)




Constraint

[x]

c(x)

([x])


















X119896 = 119891 (X119896minus1U119896) + 119908119896 (16)



Z119896 =H (X119896M) + 119903119896 (17)



















([119906119894119901119903119890][V119894119901119903119890]) = ([119888119906] + [119891] [119896119906] [L119888119894 (119909)][L119888119894 (119911)][119888V] + [119891] [119896V] [L119888119894 (119910)][L119888119894 (119911)]) (26)









OdoGyro

Camera

StaticFusion

DisplacementModel

BuildICSPs

DataAssociation

Map

ImageProcess


Solver

Update

Motion

Observation

uk Xk

Xlowastk

Xlowastkminus1






















0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle


minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)











0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia



[x] [x]

[x] cap [y] [x] ⊔ [y]

[y] [y]


[x]

f([x])

f[]([x])

IRm IRn





119898 119891 ([x]) sub 119891[] ([x]) (8)




Constraint

[x]

c(x)

([x])


















X119896 = 119891 (X119896minus1U119896) + 119908119896 (16)



Z119896 =H (X119896M) + 119903119896 (17)



















([119906119894119901119903119890][V119894119901119903119890]) = ([119888119906] + [119891] [119896119906] [L119888119894 (119909)][L119888119894 (119911)][119888V] + [119891] [119896V] [L119888119894 (119910)][L119888119894 (119911)]) (26)









OdoGyro

Camera

StaticFusion

DisplacementModel

BuildICSPs

DataAssociation

Map

ImageProcess


Solver

Update

Motion

Observation

uk Xk

Xlowastk

Xlowastkminus1






















0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle


minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)











0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia









X119896 = 119891 (X119896minus1U119896) + 119908119896 (16)



Z119896 =H (X119896M) + 119903119896 (17)



















([119906119894119901119903119890][V119894119901119903119890]) = ([119888119906] + [119891] [119896119906] [L119888119894 (119909)][L119888119894 (119911)][119888V] + [119891] [119896V] [L119888119894 (119910)][L119888119894 (119911)]) (26)









OdoGyro

Camera

StaticFusion

DisplacementModel

BuildICSPs

DataAssociation

Map

ImageProcess


Solver

Update

Motion

Observation

uk Xk

Xlowastk

Xlowastkminus1






















0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle


minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)











0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia













([119906119894119901119903119890][V119894119901119903119890]) = ([119888119906] + [119891] [119896119906] [L119888119894 (119909)][L119888119894 (119911)][119888V] + [119891] [119896V] [L119888119894 (119910)][L119888119894 (119911)]) (26)









OdoGyro

Camera

StaticFusion

DisplacementModel

BuildICSPs

DataAssociation

Map

ImageProcess


Solver

Update

Motion

Observation

uk Xk

Xlowastk

Xlowastkminus1






















0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle


minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)











0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia



OdoGyro

Camera

StaticFusion

DisplacementModel

BuildICSPs

DataAssociation

Map

ImageProcess


Solver

Update

Motion

Observation

uk Xk

Xlowastk

Xlowastkminus1






















0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle


minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)











0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia



0 2 4 6 8 10minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

1 2

3

4

5

Cone uncertainty

x (m)

y (m

)

LandmarkVehicle


minus5

minus4

minus3

minus2

minus1

0

1

2

3

4

5

y (m

)

LandmarkVehicle

0 2 4 6 8 10

1 2

3

4

5

Cone uncertainty

x (m)











0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia



0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

0 5 10minus5

0

5

Land

mar

k 1

Land

mar

k 2

Land

mar

k 5


= 0∘ = 221∘ = 539∘ = 1352∘

= 0∘ = 128∘ = 307∘ = 746∘

= 0∘ = 038∘ = 092∘ = 225∘


0 10 20 30 40 50 60 70 80 900

05

1

15

2

25

3

35

4

step number



Unc

erta

inty

(Ｇ3)





5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia



5 10 15 20 25 30 35 400

5

10

15

20

25

Step number

Aver

age p

ositi

on u

ncer

tain

ty (Ｇ

3)

= 001 = 010

= 020























RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia











RSSE119896 = radic1198642119896 (119909) + 1198642119896 (119910) (33)





minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia




minus505

1015202530

x (m)

y (m

)


minus50

minus40

minus30

minus20

minus10 0 10

minus20

minus10

0

10

20

30

4 5

1 2

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

minus50

minus40

minus30

minus20

minus10 0 10

x (m)

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

minus20

minus10

0

10

20

30

y (m

)

6

3



0 500 1000 1500 2000 2500 30000

5

10

15

20

step number

unce

rtai

nty

(Ｇ2)


0 500 1000 1500 2000 2500 30000

1

2

3

4

5

step number

unce

rtai

nty

(Ｇ2)

times10minus4









minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia




minus12

minus11

minus10

minus9

minus8

minus7

285286

287

290

291

292

293

294

295

296

297

299300

301

302

305

310

572

573

574

575

x (m)

y (m

)

Landmarkposition

Reference path



minus15

minus10

minus5

0

5

10

15

20

25

30

t=100

t=200t=300

t=400

t=500

t=60

0

START


Localization box

x (m)

y (m

)




t=200 x -2232 -2272 [-2235 -2226]y 2517 2523 [2514 2522]

t=400 x -4923 -4813 [-4929 -4918]y -251 -231 [-263 -245]

End x 014 189 [010 018]y -326 -523 [-332 -321]


RSSE

(m)

35

3

2

25

15

1

05

0


step number0 100 200 300 400 500 600


0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ex (m

)

Interval error of x

0 100 200 300 400 500 600

minus02

minus01

0

01

02

03

step number

ey (m

)

Interval error of y





minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia




minus15

minus10

minus5

0

5

10

15

20

25

30


x (m)

y (m

)

START

(a) Sequence 2



minus15

minus10

minus5

0

5

10

15

20

25

30

x (m)

y (m

)

START

(b) Sequence 3


0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(a) Width of 119864119896(119909)

0 100 200 300 400 500 600 700 8000

01

02

03

04

05

06

07

Step number

Wid

th o

f int

erva

l err

or (m

)


(b) Width of 119864119896(119910)











6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia






6 Conclusion




Data Availability




References














































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia




































RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia




RoboticsJournal of




VLSI Design



Shock and Vibration







Journal of



Volume 2018



Volume 2018


Journal of




SensorsJournal of



RotatingMachinery





Propagation






Hindawi


Advances in

Multimedia


Documents

A Low-Cost Consistent Vehicle Localization Based on ...downloads.hindawi.com/journals/jat/2018/2713729.pdf · ResearchArticle A Low-Cost Consistent Vehicle Localization Based on Interval