# Localization of a Vehicle: A Dynamic Interval Constraint Satisfaction Problem-Based Approach

Authors: Kangni Kueviakoe; Zhan Wang; Alain Lambert; Emmanuelle Frenoux; Philippe Tarroux
Journal: Journal of Sensors
 (2018)

---

## Abstract

This paper introduces a new interval constraint propagation (ICP) approach dealing with the real-time vehicle localization problem. Bayesian methods like extended Kalman filter (EKF) are classically used to achieve vehicle localization. ICP is an alternative which provides guaranteed localization results rather than probabilities. Our approach assumes that all models and measurement errors are bounded within known limits without any other hypotheses on the probability distribution. The proposed algorithm uses a low-level consistency algorithm and has been validated with an outdoor vehicle equipped with a GPS receiver, a gyro, and odometers. Results have been compared to EKF and other ICP methods such as hull consistency (HC4) and 3-bound (3B) algorithms. Both consistencies of EKF and our algorithm have been experimentally studied.


---

## Body

## 1. Introduction

This paper deals with outdoor vehicle ego-localization. Localization is part of many automotive applications where safety is of crucial importance. This estimation problem, which involves fusion of data from dead reckoning and absolute sensors, is generally solved by Bayesian approaches [1, 2]. Interval analysis-based methods are alternative and less known methods. These methods use either set inversion [3, 4] or constraint propagation techniques [5, 6] to deal with the localization problem and provide a set of intervals containing the vehicle pose (position and orientation) rather than probabilities.Most of the early published papers on constraint programming can be dated back to the 1970s. They have mainly dealt with discrete domains [7]. In the 1980s, Gallaire [8] and Jaffar and Lassez [9] noted that logic programming can be seen as a particular type of constraint programming. Both logic programming and constraint programming imply that the user states what has to be solved instead of how to solve it. Many combinatorial problems such as scheduling and timetabling were then formulated in the form of constraint satisfaction problems (CSP).Constraint propagation is the most used technique for solving CSPs. It combines consistent techniques and systematic search methods. Arc consistency could be achieved by the AC-3 (“AC” stands for “arc consistency”) algorithm [7] or by many other algorithms (like AC-5 [10], AC-6 [11], etc.) using only binary CSPs (each constraint involves at most two variables). However, many real-life problems can be naturally modeled as nonbinary CSPs: the constraint involves more than two variables [12]. Two approaches were developed to deal with nonbinary CSPs. The first one translates a nonbinary CSP into several binary CSPs [13, 14]. The second kind of approach directly deals with nonbinary constraints. For example, GAC-4 [15] is a generalization of AC-4 to nonbinary constraints.In 1987, Cleary [16] and Davis [17] pioneered the use of constraint propagation for interval analysis. In 1989, a generalized constraint propagation scheme based on interval arithmetic was designed by Hyvönen [18]. Cleary [16] proposed a narrowing algorithm which was improved by Benhamou and Older [19]. Later, that algorithm was renamed as HC3 [20] because it is very close to the classical AC-3 algorithm. Next, Benhamou et al. proposed that HC4 [20] does not need constraints decomposition but it encounters the well-known dependency problem. To cope with this problem, BC3 [21] was developed. It overcomes the dependency problem at the cost of a huge computing time. BC4 [20] merges both BC3 and HC4 and reduces the computation time. BC5 [22], by using the interval Gauss-Seidel method, further reduces the computing time. All the preceding methods (HC4, BC3, BC4, and BC5) work with one constraint at a time. They compute the variables of a constraint locally without taking into account the whole constraint system. They are then named “local consistency algorithms.” By then, they do not reach the optimal contraction for all the domains of variables of the problem. This leads to a problem called “locality problem.” To take into account all the constraints of a given problem at a time, strong consistency algorithms such as the 3B algorithm [23] have been proposed. Strong consistency algorithms seek the strength of the domain contractions rather than the shortness of computation time.Interval constraint propagation (ICP) was used to solve the robotic localization problems by Gning and Bonnifait [6, 24] more than ten years ago. It was mainly used for outdoor vehicle localization [25, 26] and underwater robot localization [3, 5]. Those works use a forward-backward propagation technique based on primitive constraints (following the principle of the Waltz algorithm [27]). Drevelle and Bonnifait [28] use a relaxed constraint propagation approach to deal with erroneous GPS measurements. Lassoued and Bonnifait [29] combine constraint propagation and set inversion techniques and present a cooperative localization method with significant enhancement in terms of accuracy and confidence domains. Some other researchers propose specific contractors [30] or separators [31] to solve the localization problem. Tubes are used in scenarios where the system is described by a continuous time state equation. This tool allows to both localize the robot and correct its clock. The readers might refer to [32, 33] to see more details about the application of tubes in the robotic area. The main advantage of ICP over Bayesian algorithms is that it guarantees that the position of the robot is contained in a box. Bayesian algorithms [1] can only associate a probability to such a box. Consequently, safety cannot be guaranteed by Bayesian algorithms.A previous work [34] demonstrated that classical forward-backward ICP do not reduce the heading imprecision for a vehicle equipped with a GPS receiver, a gyro, and odometers. Another study [35] concluded that various ICP algorithms ([16, 20, 21, 23]) have the same drawback. Only the 3B consistency algorithm [23] (based on a strong consistency technique) can reduce the heading imprecision of the vehicle during its displacement. The price to pay is an increase in the computing time.We improved previous works by proposing a new ICP method for vehicle localization with heading correction and real-time capabilities. The proposed algorithm employs HC4 as a low-level algorithm. It is easier to be used than previous ones as it does not need to decompose all the constraints into elementary/binary constraints. Our algorithm has been validated with an outdoor vehicle equipped with a GPS receiver, a gyro, and odometers. The results obtained by our method are compared with three algorithms by using a very accurate positioning system as ground truth. This paper includes and extends results previously presented in [36].Section2 introduces constraint propagation and interval analysis. Two ICP algorithms (HC4 and 3B) are discussed in Section 3. A new localization problem formalization and its solving are presented in Section 4 while Section 5 shows our experimental results.


## 2. Overview of Interval Analysis and Constraint Propagation

Interval analysis and constraint propagation are the two main mathematical concepts on which our work is based. A brief overview of these methods is given in this section.


### 2.1. Interval Analysis

Interval analysis was introduced in the 1960s by Moore [37–39] in order to deal with approximation problems encountered during calculation. The main idea is to represent numbers by intervals which include their values. An interval x is a connected subset of R, defined by its lower bound x¯ and upper bound x¯. The following notations are used as shorthands to denote closed/open interval: x¯,x¯=x∈R∣x¯≤x≤x¯ and x¯,x¯=x∈R∣x¯≤x<x¯. When no confusion may arise, x is used to represent an interval whether it is closed or not. The width of a nonempty interval x is defined by wx=x¯−x¯.Rules have been defined to perform every usual arithmetical operation on intervals [38, 39]. Considering two intervals x and y and a binary operator ⋄∈+,−,×,÷, the smallest interval which contains all feasible values for x⋄y is defined as
(1)x⋄y=x⋄y∣x∈x,y∈y.The set-theoretic operations can be applied to intervals. Theintersection of two intervals is defined by
(2)x∩y=z∈Rz∈x,z∈y,which is always an interval. This is not the case for their union
(3)x∪y=z∈R∣z∈xorz∈y.Consequently, aninterval hull is defined as the smallest interval that contains a subset X of R. For example, the interval hull of 2,4∪5,8 is the interval 2,8. The interval hull of the union of x and y is denoted by x⊔y.


### 2.2. Interval Extension

We often need to compute the image of a number by a function. One purpose of interval analysis is to provide, for a large variety of functionsf, interval extensions f (also called “inclusion function”) that can be evaluated reasonably quickly and such that fx is not too large. For any function f defined with arithmetical operators and elementary functions,
(4)fx⊂fx,∀x∈R.To create an inclusion function, it is possible to replace all the variables by their intervals and then all the included operators by their interval equivalents. The resulting interval function is called a “natural inclusion function.” For instance,(5)fx=4x4−7lnx−3,∀x∈Rhas the natural inclusion function
(6)fx=4x4−7lnx−3,∀x∈R.An interval extension can be used to represent an equation based on interval variables. Such equations, also called constraints, are the core of an interval-based constraint satisfaction problem which is covered by the next subsection.


### 2.3. Contractor

The concept of the contractor is directly inspired by the ubiquitous concept of filtering algorithms in constraint programming. Given a constraintc relating a set of variable x, an algorithm C is called a contractor (or filtering algorithm) if C returns a subdomain of the input domain x and the resulting subdomain Cx contains all the feasible points with respect to the constraint c:
(7)Cx⊆x,cx⇒x∈Cx,∀x∈x.


### 2.4. Interval Constraint Satisfaction Problem (ICSP)

The concept of interval constraint satisfaction problem (ICSP) was introduced by Hyvönen [40] in 1992. An interval constraint satisfaction problem (ICSP) or interval CSP is a mathematical problem solved by finding states satisfying the constraints. An ICSP is defined by
(i)
a setV of variables v1,v2,…,vn,
(ii)
a setD of domains D1,D2,…,Dn, such as for each variable vi, a domain Di with the possible values for that variable is given and Di is an interval or union of intervals;
(iii)
a setC of p constraints c1,c2,…,cp, defining the relations between a subset of variables, restricting the possible solutions.Hyvönen et al. proposed, in 1993, “INC++” [41], a basic interval processing tool which allows evaluating interval functions and aims at solving ICSP. Many other interval solvers such as Numerica [42], RealPaver [43], ALIAS [44], ICOS [45], GloptLab [46], and IBEX (IBEX is available at http://www.ibex-lib.org/) [47, 48] have been designed so far.Interval constraint propagation (ICP) consists in iterating domain reductions, by using the set ofp constraints, until no domain can be contracted. Many kinds of CSP have been introduced in order to express specific situations. The concept of “dynamic CSP” (DCSP) was introduced by Dechter [49]. It represents a CSP that is evolving or changing with the time. This means that at any time, new constraints can be added to the constraint network and other constraints can be removed from the CSP. The addition (resp. deletion) operation is also known as “restriction” (resp. “relaxation”). An interval CSP which is dynamic is consequently a “dynamic ICSP” or DICSP for short.


### 2.5. Interval Vector

Interval vector is a generalization of the interval’s concept: an interval vector, also called “a box,” has intervals as components. It is a Cartesian product ofn intervals. The width of an interval vector wx=maxi=1,…,nwxi can be computed in order to estimate its size.A vehicle pose is represented by 3 parameters:x-axis and y-axis values and the heading information θ. Consequently, for vehicle localization, the solution is a three-dimensional interval vector. Interval constraint propagation (ICP) algorithms process interval vectors as the variables of an interval constraint satisfaction problem. We use these ICP algorithms to solve the vehicle localization problem which we stated as an ICSP. The next section will present two ICP algorithms.


## 2.1. Interval Analysis

Interval analysis was introduced in the 1960s by Moore [37–39] in order to deal with approximation problems encountered during calculation. The main idea is to represent numbers by intervals which include their values. An interval x is a connected subset of R, defined by its lower bound x¯ and upper bound x¯. The following notations are used as shorthands to denote closed/open interval: x¯,x¯=x∈R∣x¯≤x≤x¯ and x¯,x¯=x∈R∣x¯≤x<x¯. When no confusion may arise, x is used to represent an interval whether it is closed or not. The width of a nonempty interval x is defined by wx=x¯−x¯.Rules have been defined to perform every usual arithmetical operation on intervals [38, 39]. Considering two intervals x and y and a binary operator ⋄∈+,−,×,÷, the smallest interval which contains all feasible values for x⋄y is defined as
(1)x⋄y=x⋄y∣x∈x,y∈y.The set-theoretic operations can be applied to intervals. Theintersection of two intervals is defined by
(2)x∩y=z∈Rz∈x,z∈y,which is always an interval. This is not the case for their union
(3)x∪y=z∈R∣z∈xorz∈y.Consequently, aninterval hull is defined as the smallest interval that contains a subset X of R. For example, the interval hull of 2,4∪5,8 is the interval 2,8. The interval hull of the union of x and y is denoted by x⊔y.


## 2.2. Interval Extension

We often need to compute the image of a number by a function. One purpose of interval analysis is to provide, for a large variety of functionsf, interval extensions f (also called “inclusion function”) that can be evaluated reasonably quickly and such that fx is not too large. For any function f defined with arithmetical operators and elementary functions,
(4)fx⊂fx,∀x∈R.To create an inclusion function, it is possible to replace all the variables by their intervals and then all the included operators by their interval equivalents. The resulting interval function is called a “natural inclusion function.” For instance,(5)fx=4x4−7lnx−3,∀x∈Rhas the natural inclusion function
(6)fx=4x4−7lnx−3,∀x∈R.An interval extension can be used to represent an equation based on interval variables. Such equations, also called constraints, are the core of an interval-based constraint satisfaction problem which is covered by the next subsection.


## 2.3. Contractor

The concept of the contractor is directly inspired by the ubiquitous concept of filtering algorithms in constraint programming. Given a constraintc relating a set of variable x, an algorithm C is called a contractor (or filtering algorithm) if C returns a subdomain of the input domain x and the resulting subdomain Cx contains all the feasible points with respect to the constraint c:
(7)Cx⊆x,cx⇒x∈Cx,∀x∈x.


## 2.4. Interval Constraint Satisfaction Problem (ICSP)

The concept of interval constraint satisfaction problem (ICSP) was introduced by Hyvönen [40] in 1992. An interval constraint satisfaction problem (ICSP) or interval CSP is a mathematical problem solved by finding states satisfying the constraints. An ICSP is defined by
(i)
a setV of variables v1,v2,…,vn,
(ii)
a setD of domains D1,D2,…,Dn, such as for each variable vi, a domain Di with the possible values for that variable is given and Di is an interval or union of intervals;
(iii)
a setC of p constraints c1,c2,…,cp, defining the relations between a subset of variables, restricting the possible solutions.Hyvönen et al. proposed, in 1993, “INC++” [41], a basic interval processing tool which allows evaluating interval functions and aims at solving ICSP. Many other interval solvers such as Numerica [42], RealPaver [43], ALIAS [44], ICOS [45], GloptLab [46], and IBEX (IBEX is available at http://www.ibex-lib.org/) [47, 48] have been designed so far.Interval constraint propagation (ICP) consists in iterating domain reductions, by using the set ofp constraints, until no domain can be contracted. Many kinds of CSP have been introduced in order to express specific situations. The concept of “dynamic CSP” (DCSP) was introduced by Dechter [49]. It represents a CSP that is evolving or changing with the time. This means that at any time, new constraints can be added to the constraint network and other constraints can be removed from the CSP. The addition (resp. deletion) operation is also known as “restriction” (resp. “relaxation”). An interval CSP which is dynamic is consequently a “dynamic ICSP” or DICSP for short.


## 2.5. Interval Vector

Interval vector is a generalization of the interval’s concept: an interval vector, also called “a box,” has intervals as components. It is a Cartesian product ofn intervals. The width of an interval vector wx=maxi=1,…,nwxi can be computed in order to estimate its size.A vehicle pose is represented by 3 parameters:x-axis and y-axis values and the heading information θ. Consequently, for vehicle localization, the solution is a three-dimensional interval vector. Interval constraint propagation (ICP) algorithms process interval vectors as the variables of an interval constraint satisfaction problem. We use these ICP algorithms to solve the vehicle localization problem which we stated as an ICSP. The next section will present two ICP algorithms.


## 3. ICP Algorithms

In this section, we introduce two interval constraint propagation (ICP) algorithms. Previous studies [35] have shown that HC4 is the fastest one and 3B is the one that gives the best contraction of domains. We do not introduce BC5 which is slower than HC4 and unnecessary because of no dependency inside our constraints.


### 3.1. The HC4 Algorithm

HC4 [20] enforces hull consistency over complex constraints without decomposing them into primitive ones. Hull consistency [20] is ensured, for a constraint c, with regard to an interval vector B, if and only if
(8)B=hull□B∩ρc,where hull□ is a function computing an approximation of the smallest interval vector containing its argument and ρc denotes the underlying relation of the given constraint c. If P is an ICSP involving an interval vector B, P is hull consistent if each constraint c of P is hull consistent. Consequently, B cannot be reduced anymore by applying c. Hull consistency is also called 2B consistency [50]. The interval vector B is the Cartesian product of n intervals: B=I1×⋯×In. HC4 follows a loop propagation and processes constraints individually using the HC4 revise function. HC4 revise reduces the domains by removing inconsistent values and returns the new narrowed domains to HC4.HC4 revise uses a binary tree representation of constraints, where leaves are constants or variables and nodes represent elementary operation symbols such as/, ×, and log. HC4 revise works in two phases:
(i)
The “forward evaluation phase” goes through the tree, from the leaves to the root. It evaluates recursively the interval of subexpression represented by the current node using the natural extensions of the underlying functions.
(ii)
The “backward propagation phase” traverses the tree from the root to the leaves applying on each node a contraction operator (a projection). The contraction operator narrows the interval of the current node by removing inconsistent values.The main limitation of the hull consistency algorithms (HC4 and HC3) is their sensitivity to multiple occurrences of variables [21]. This is referred as the dependency problem. When considering, for instance, the constraint x×x+y2=2, with x,y∈−2,4×−1,1, hull consistency algorithms would not reduce the initial box, but would perfectly reduce it when the constraint is stated like x2+y2=2.The 3-bound consistency algorithm overcomes the dependency problem and the locality problem as described in Section1.


### 3.2. The 3B Algorithm

3B algorithm [23] also referred as the “strong consistency algorithm” computes the projection of the sets of constraints over the variables: it combines constraints in order to improve the precision of domain narrowing. The principle of 3B is that a variable is instantiated to one of its interval bounds and the remaining problem is solved in a hull or box consistency way: 3B uses a low level algorithm (for instance BC4) in order to contract the intervals. Let P be an ICSP, x a variable of P, and Dx=x¯,x¯ the domain of x. Dx is 3B consistent if and only if P′ and P″ are both nonempty where P′=ΦP∪x=x¯ and P″=ΦP∪x=x¯ with ΦP denoting the enclosure of P. P is 3B consistent if all the domains of variables are 3B consistent.Example 1.
Let us consider that the constraints ofP is given as follows:
(9)x+y=2,y≤x+1,y≥x,with Dx=0.5,1 and Dy=1,1.5. For each instantiation of x, we have a 2B-consistent subproblem to solve (see Table 1). For each line of Table 1, the solution domain is not empty, which indicates that the domain of variable x is 3B consistent. The same method is used for the variable y and ultimately proves that the ICSP is 3B consistent.
For further information about 3B consistency methods, the reader might refer to [23].Table 1
Establishing 3B consistency.
Instantiation
Constraint
Solution
Empty
P′=ΦP∪x=0.5
x+y=2y≤x+1y≥xx=0.5
x=0.5y=1.5
Non
P″=ΦP∪x=1
x+y=2y≤x+1y≥xx=0.5
x=1y=1
NonAfter presenting the mathematical tools required to understand our approach, let us now deal with the localization problem itself.


## 3.1. The HC4 Algorithm

HC4 [20] enforces hull consistency over complex constraints without decomposing them into primitive ones. Hull consistency [20] is ensured, for a constraint c, with regard to an interval vector B, if and only if
(8)B=hull□B∩ρc,where hull□ is a function computing an approximation of the smallest interval vector containing its argument and ρc denotes the underlying relation of the given constraint c. If P is an ICSP involving an interval vector B, P is hull consistent if each constraint c of P is hull consistent. Consequently, B cannot be reduced anymore by applying c. Hull consistency is also called 2B consistency [50]. The interval vector B is the Cartesian product of n intervals: B=I1×⋯×In. HC4 follows a loop propagation and processes constraints individually using the HC4 revise function. HC4 revise reduces the domains by removing inconsistent values and returns the new narrowed domains to HC4.HC4 revise uses a binary tree representation of constraints, where leaves are constants or variables and nodes represent elementary operation symbols such as/, ×, and log. HC4 revise works in two phases:
(i)
The “forward evaluation phase” goes through the tree, from the leaves to the root. It evaluates recursively the interval of subexpression represented by the current node using the natural extensions of the underlying functions.
(ii)
The “backward propagation phase” traverses the tree from the root to the leaves applying on each node a contraction operator (a projection). The contraction operator narrows the interval of the current node by removing inconsistent values.The main limitation of the hull consistency algorithms (HC4 and HC3) is their sensitivity to multiple occurrences of variables [21]. This is referred as the dependency problem. When considering, for instance, the constraint x×x+y2=2, with x,y∈−2,4×−1,1, hull consistency algorithms would not reduce the initial box, but would perfectly reduce it when the constraint is stated like x2+y2=2.The 3-bound consistency algorithm overcomes the dependency problem and the locality problem as described in Section1.


## 3.2. The 3B Algorithm

3B algorithm [23] also referred as the “strong consistency algorithm” computes the projection of the sets of constraints over the variables: it combines constraints in order to improve the precision of domain narrowing. The principle of 3B is that a variable is instantiated to one of its interval bounds and the remaining problem is solved in a hull or box consistency way: 3B uses a low level algorithm (for instance BC4) in order to contract the intervals. Let P be an ICSP, x a variable of P, and Dx=x¯,x¯ the domain of x. Dx is 3B consistent if and only if P′ and P″ are both nonempty where P′=ΦP∪x=x¯ and P″=ΦP∪x=x¯ with ΦP denoting the enclosure of P. P is 3B consistent if all the domains of variables are 3B consistent.Example 1.
Let us consider that the constraints ofP is given as follows:
(9)x+y=2,y≤x+1,y≥x,with Dx=0.5,1 and Dy=1,1.5. For each instantiation of x, we have a 2B-consistent subproblem to solve (see Table 1). For each line of Table 1, the solution domain is not empty, which indicates that the domain of variable x is 3B consistent. The same method is used for the variable y and ultimately proves that the ICSP is 3B consistent.
For further information about 3B consistency methods, the reader might refer to [23].Table 1
Establishing 3B consistency.
Instantiation
Constraint
Solution
Empty
P′=ΦP∪x=0.5
x+y=2y≤x+1y≥xx=0.5
x=0.5y=1.5
Non
P″=ΦP∪x=1
x+y=2y≤x+1y≥xx=0.5
x=1y=1
NonAfter presenting the mathematical tools required to understand our approach, let us now deal with the localization problem itself.


## 4. The Localization Problem

### 4.1. Problem Statement

The localization problem consists of estimating in a continuous way the position and heading information of an autonomous mobile system. Figure1 depicts a localization situation. The goal is to determine the current pose xk, yk, and θk knowing the previous pose xk−1, yk−1, and θk−1. It is a state estimation of a nonlinear dynamic system. Our vehicle is equipped with two proprioceptive sensors (gyro and odometers) and an exteroceptive sensor (GPS receiver). The elementary displacement and rotation (δsk and δθk) are given by a static fusion process which involves (see [24, 26] for more details) (10), (11), and (12). Then, they can be directly used in a dynamic fusion process (Algorithm 1) to estimate the vehicle location when a GPS measurement is available.Figure 1
Vehicle localization problem.


#### 4.1.1. Gyro

A gyro is a heading sensor which measures the rotational speed in an inertial reference system. It uses the Coriolis force to output a yaw rate from which we deduce the elementary rotationδθgyro and its associated interval δθ:
(10)δθ=δθgyro−εgyro,δθgyro+εgyro,where εgyro is the error on δθ.


#### 4.1.2. Odometers

Odometers are set on both rear wheels of our vehicle. They give the distance traveled by each wheel independently. The accuracy of an odometer relies on its number of steps and its maximum error. In our case, the maximum error is known to be one step. Thus the value of the displacement of a nonsliding wheel can be bounded byδp=δpodo−1,δpodo+1 where δpodo stands for the number of measured steps. Then, the movement of a sliding wheel can be deduced from a nonsliding one by adding a sliding noise εodo. Consequently, the displacement considering a sliding wheel is defined by
(11)δp=δpodo−1−εodo,δpodo+1+εodo.The elementary displacementδsk of the vehicle is then obtained by
(12)δsk=πwlδpl+wrδprP,where wr and wl, respectively, stand for the radius of the right and left wheel and P represents the odometer resolution.


#### 4.1.3. GPS Receiver

The Global Positioning System (GPS) is a global navigation satellite system that provides geolocation and time information to a GPS receiver anywhere on or near the Earth. When a GPS receiver reads the transmission of three or more satellites, it calculates the arrival time differences and its relative distance to each satellite. Our GPS receiver performs the necessary calculation and returns latitude and longitude coordinates which are converted as a positiony=x,yT in a Cartesian local frame. Furthermore, the GPS receiver computes the measurement imprecision εgps on-line and sends it into the GST NMEA frame.
(13)y=xgps−εx,gps,xgps+εx,gpsygps−εy,gps,ygps+εy,gps.For further information about sensors, the reader might refer to [51].Once we have constituted the intervals from sensor data, we can define a nonlinear dynamic-bounded error model to build constraints of a DICSP, as presented in the next section.


### 4.2. A Dynamic Interval Constraint Satisfaction Problem (DICSP)

We propose to define the localization problem as a dynamic interval constraint satisfaction problem: at timek, a new ICSPk (Vk, Dk, Ck) is generated and defined as follows:
(i)
a set of variables,Vk,
(14)Vk=xk,yk,θk,xk−1,yk−1,θk−1,δsk,δθk,
(ii)
a set of domains,Dk,
(15)Dk=xk,yk,θk,xk−1,yk−1,θk−1,δsk,δθk,
(iii)
a set of constraints,Ck,
(16)Ck=Ck1,Ck2,Ck3T,(17)Ck1⇔xk=xk−1+δskcosθk−1+δθk2,Ck2⇔yk=yk−1+δsksinθk−1+δθk2,Ck3⇔θk=θk−1+δθk,where Ck represents the set of constraints deduced from the vehicle displacement model proposed by Seignez et al. [52]. The domains of variables are defined as follows:
(i)
δsk and δθk are given by the odometer and gyro measurements thanks to (12) and (10).
(ii)
xk and yk are initialized with GPS measurements (see (13)).
(iii)
θk is initialized to −π,π.
(iv)
xk−1, yk−1, and θk−1 are obtained from the solution of the previous CSP (at time k−1).We consider all the state equations from timek−Ξ+1 to time k (where k is the current time and Ξ is the window size): the contractions are performed in a sliding window of length Ξ. In other words, we use Ξ sets of equations (current and past information) in our interval constraint satisfaction problem in order to localize the vehicle. At each time k, we add a new constraint set Ck to the existing constraint sets. If k>Ξ, we delete the oldest constraint set Ck−Ξ from it. The addition (resp. deletion) operation is also known as “restriction” (resp. “relaxation”).The localization DICSP (dynamic ICSP) keeps the same number of constraints (3×Ξ) but changes temporally. The localization DICSP is defined at time k by
(18)DICSPk=ICSPkICSPk−1ICSPk−2⋯ICSPk−Ξ+1.ICP algorithms allow us to solveDICSPk. For our experiment, some local consistency algorithms including HC4 and the strong consistency 3B algorithm have been used.We compared many ICP algorithms for solving our DICSPs. The best results are obtained by the 3B algorithm [35]. Unfortunately, 3B is too slow to work in a real-time environment. And consequently, it cannot be used to solve our real-time problem. We noticed that apart from 3B, no algorithms can contract the θ parameter [36], as explained detailedly in the next section.


### 4.3. Uncorrected Yaw Problem

Vincke and Lambert [34] had shown that classical interval propagation algorithms can not reduce the heading uncertainty of a vehicle equipped with a GPS receiver, a gyro, and odometers. Let us consider a simple forward-backward propagation algorithm, the contraction of the heading uncertainty is performed by (19), which is deduced from (17) in a backward way.
(19)θk−1=θk−1⋂arccosxk−xk−1δsk−δθk2,θk−1=θk−1⋂arcsinyk−yk−1δsk−δθk2.Example 2.
Considering the case study with a straight displacement along thex-axis, the orientation variation is null −δθk/2=0. The correction on θ is realized by arcsinyk−yk−1/δsk and arccosxk−xk−1/δsk.
The width ofyk−1 is around 10 m in most of our experiments: yk−1=−5,5. Thus, we can get
(20)yk−yk−1=−5+ϵy,5−ϵy−−5,5=−10+ϵy,10−ϵy,where ϵy is a small reduction of the imprecision due to a new GPS measurement. δsk is around 1 m:
(21)δsk=1−ϵs,1+ϵs.
Finally,(22)arcsinyk−yk−1δsk=arcsin−10+ϵy,10−ϵy1−ϵs,1+ϵs=−π,π.
The value inside thearcsin function is far beyond −1,1, leading to a full ignorance on the orientation. The same reasoning leads to the same result for the arccos function. Thus, (19) is unable to correct the orientation.During the vehicle’s displacement, the interval representing the heading uncertainty will grow continuously although other parameters of the pose are contracted. It is a big issue because the heading information is important for a good estimation of the vehicle’s position on thex-axis and y-axis. Reference [34] explained the problem but gives no solution. Let us notice that their approach needed to decompose all the constraints of the problem into elementary/binary constraints.The uncorrected yaw problem is observed only with local consistency algorithms which solve constraints one by one. They are subject to the “locality problem”: the solution is always local with regard to one constraint at a time. Only a strong consistency algorithm (3B for instance) can overcome this problem and correct the yaw. Unfortunately, strong consistency algorithms have a huge computing time and are not suited for real-time issue [53]. We propose to solve this problem by introducing a new algorithm which splits the θ parameter and creates multiple DICSP (called νDICSP). The next section details our method.


### 4.4. A Multiple Dynamic Interval Constraint Satisfaction Problem

In order to correct the yaw, we propose to split the variableθk−Ξ into ν variables θk−Ξi,i∈1,2,…,ν where ν stands for the “splitting factor.”Each domainθk−Ξi is then defined by
(23)θk−Ξi=θk−Ξ¯+i−1S¯,θk−Ξ¯+iS¯,where k≥Ξ, S=wθk−Ξ/v is the width of each θk−Ξi, and wθk−Ξ represents the width of the interval θk−Ξ. S¯ and S¯ are, respectively, the round down and the round up of the number S.Example 3.
To illustrate, let us considerθk−Ξ=1,2. If ν=10 is the “splitting factor,” then, the following intervals will be generated: θk−Ξ1=1¯,1.1¯,θk−Ξ2=1.1¯,1.2¯,θk−Ξ3=1.2¯,1.3¯,…,θk−Ξ10=1.9¯,2¯.In the first step of the algorithm,k<Ξ and θk−Ξ is undefined: the splitting is realized on θ0. θk−Ξ is the union of all the θk−Ξi:
(24)θk−Ξ=⋃i=1vθk−Ξi.Other domains remain unchanged (unsplitted). The proposed splitting leads to aνDICSP:
(25)νDICSPk=DICSPk1,DICSPk2,…,DICSPkν,with
(26)DICSPki=ICSPki,ICSPk−1i,…,ICSPk−Ξ+1iT.Algorithm 1: DICPS.
Ensure:DICSPk
(1) (θk−Ξ1,…,θk−Ξv) ← Split(θk−Ξ,v)
(2)fori ← 1 : vdo
(3)DICSPki ← Build(DICSPk,θk−Ξ1)
(4)  [Xki] ← Solve(DICSPki)
(5)end for
(6) [Xres] ← Hull(Xk1,…,Xkv)
(7)return [Xres]The DICSPs (columns of theνDICSP) are separately solved by a local consistency algorithm. The solution of the localization problem is given by applying the interval hull function on the results obtained from the first row of the νDICSPk. It defines the current position and orientation of the vehicle as an interval vector xk,yk,θk. When new measurements are available, the next localization process starts by applying the interval hull function on the results of the last row of the νDICSPk (first step before splitting).Sometimes, one (or more) DICSP does not get a solution: one of the domains is empty. This is an expected behavior that enables fast correction by eliminating a whole set of erroneous pose.The building and the solving of the multiple dynamic interval constraint satisfaction problem are realized by our proposed dynamic interval constraint propagation with the splitting algorithm (DICSP, see Algorithm1), explained hereafter:
(i)
Line 1: the oldest variableθk−Ξ, representing the orientation in the initial DICSP, is split into ν intervals.
(ii)
Line 3: for eachθk−Ξi, a DICSP is generated.
(iii)
Line 4: each DICSP is submitted to a solving process (by an ICP algorithm).
(iv)
Line 6: the new domains, resulting from the process of the preview step, are grouped with an intervalhull function.
(v)
Line 7: the interval hull constitutes an interval vector which is returned as the vehicle position.This algorithm has been implemented and tested on a vehicle running on a test track as explained in the next section.


### 4.5. About Completeness

“The two most appealing features of interval analysis and numerical constraint propagation, when used to solve numerical problems, are completeness and rigor. Completeness means the ability to find all solutions, whereas rigor is the ability to control the rounding errors due to floating-point computation” [54]. The rigor property is kept by our algorithm as we only used interval operators. The completeness is discussed because our algorithm splits the variables before applying classical constraint propagation techniques. We propose a new ICP algorithm with splitting: it splits an interval vector x into boxes x1,x2,…,xm, so that
(27)x=⋃i=1mxi.Then, the range of a functionf· over x, possibly defined by (17), can be evaluated by
(28)fx=f⋃i=1mxi.The bisection usually leads to an improved estimate. Our algorithm takes advantage of this property and combine it with DICSP to solve the uncorrected yaw problem by splitting the parameterθ. According to the interval extension definition given in Section 2.2, we have
(29)fx⊆⋃i=1mfxi.And thanks to the definition of interval hull (see Section2.2), we obtain
(30)fx⊆⨆i=1mfxi.Since⊔i=1mfxi is a genuine superset of the left-hand side, all correct answers are included in it. Furthermore, each term of the interval hull is solved by a local consistency algorithm which ensures that none of the solutions are removed (thus achieving the completeness of our method).


## 4.1. Problem Statement

The localization problem consists of estimating in a continuous way the position and heading information of an autonomous mobile system. Figure1 depicts a localization situation. The goal is to determine the current pose xk, yk, and θk knowing the previous pose xk−1, yk−1, and θk−1. It is a state estimation of a nonlinear dynamic system. Our vehicle is equipped with two proprioceptive sensors (gyro and odometers) and an exteroceptive sensor (GPS receiver). The elementary displacement and rotation (δsk and δθk) are given by a static fusion process which involves (see [24, 26] for more details) (10), (11), and (12). Then, they can be directly used in a dynamic fusion process (Algorithm 1) to estimate the vehicle location when a GPS measurement is available.Figure 1
Vehicle localization problem.


### 4.1.1. Gyro

A gyro is a heading sensor which measures the rotational speed in an inertial reference system. It uses the Coriolis force to output a yaw rate from which we deduce the elementary rotationδθgyro and its associated interval δθ:
(10)δθ=δθgyro−εgyro,δθgyro+εgyro,where εgyro is the error on δθ.


### 4.1.2. Odometers

Odometers are set on both rear wheels of our vehicle. They give the distance traveled by each wheel independently. The accuracy of an odometer relies on its number of steps and its maximum error. In our case, the maximum error is known to be one step. Thus the value of the displacement of a nonsliding wheel can be bounded byδp=δpodo−1,δpodo+1 where δpodo stands for the number of measured steps. Then, the movement of a sliding wheel can be deduced from a nonsliding one by adding a sliding noise εodo. Consequently, the displacement considering a sliding wheel is defined by
(11)δp=δpodo−1−εodo,δpodo+1+εodo.The elementary displacementδsk of the vehicle is then obtained by
(12)δsk=πwlδpl+wrδprP,where wr and wl, respectively, stand for the radius of the right and left wheel and P represents the odometer resolution.


### 4.1.3. GPS Receiver

The Global Positioning System (GPS) is a global navigation satellite system that provides geolocation and time information to a GPS receiver anywhere on or near the Earth. When a GPS receiver reads the transmission of three or more satellites, it calculates the arrival time differences and its relative distance to each satellite. Our GPS receiver performs the necessary calculation and returns latitude and longitude coordinates which are converted as a positiony=x,yT in a Cartesian local frame. Furthermore, the GPS receiver computes the measurement imprecision εgps on-line and sends it into the GST NMEA frame.
(13)y=xgps−εx,gps,xgps+εx,gpsygps−εy,gps,ygps+εy,gps.For further information about sensors, the reader might refer to [51].Once we have constituted the intervals from sensor data, we can define a nonlinear dynamic-bounded error model to build constraints of a DICSP, as presented in the next section.


## 4.1.1. Gyro

A gyro is a heading sensor which measures the rotational speed in an inertial reference system. It uses the Coriolis force to output a yaw rate from which we deduce the elementary rotationδθgyro and its associated interval δθ:
(10)δθ=δθgyro−εgyro,δθgyro+εgyro,where εgyro is the error on δθ.


## 4.1.2. Odometers

Odometers are set on both rear wheels of our vehicle. They give the distance traveled by each wheel independently. The accuracy of an odometer relies on its number of steps and its maximum error. In our case, the maximum error is known to be one step. Thus the value of the displacement of a nonsliding wheel can be bounded byδp=δpodo−1,δpodo+1 where δpodo stands for the number of measured steps. Then, the movement of a sliding wheel can be deduced from a nonsliding one by adding a sliding noise εodo. Consequently, the displacement considering a sliding wheel is defined by
(11)δp=δpodo−1−εodo,δpodo+1+εodo.The elementary displacementδsk of the vehicle is then obtained by
(12)δsk=πwlδpl+wrδprP,where wr and wl, respectively, stand for the radius of the right and left wheel and P represents the odometer resolution.


## 4.1.3. GPS Receiver

The Global Positioning System (GPS) is a global navigation satellite system that provides geolocation and time information to a GPS receiver anywhere on or near the Earth. When a GPS receiver reads the transmission of three or more satellites, it calculates the arrival time differences and its relative distance to each satellite. Our GPS receiver performs the necessary calculation and returns latitude and longitude coordinates which are converted as a positiony=x,yT in a Cartesian local frame. Furthermore, the GPS receiver computes the measurement imprecision εgps on-line and sends it into the GST NMEA frame.
(13)y=xgps−εx,gps,xgps+εx,gpsygps−εy,gps,ygps+εy,gps.For further information about sensors, the reader might refer to [51].Once we have constituted the intervals from sensor data, we can define a nonlinear dynamic-bounded error model to build constraints of a DICSP, as presented in the next section.


## 4.2. A Dynamic Interval Constraint Satisfaction Problem (DICSP)

We propose to define the localization problem as a dynamic interval constraint satisfaction problem: at timek, a new ICSPk (Vk, Dk, Ck) is generated and defined as follows:
(i)
a set of variables,Vk,
(14)Vk=xk,yk,θk,xk−1,yk−1,θk−1,δsk,δθk,
(ii)
a set of domains,Dk,
(15)Dk=xk,yk,θk,xk−1,yk−1,θk−1,δsk,δθk,
(iii)
a set of constraints,Ck,
(16)Ck=Ck1,Ck2,Ck3T,(17)Ck1⇔xk=xk−1+δskcosθk−1+δθk2,Ck2⇔yk=yk−1+δsksinθk−1+δθk2,Ck3⇔θk=θk−1+δθk,where Ck represents the set of constraints deduced from the vehicle displacement model proposed by Seignez et al. [52]. The domains of variables are defined as follows:
(i)
δsk and δθk are given by the odometer and gyro measurements thanks to (12) and (10).
(ii)
xk and yk are initialized with GPS measurements (see (13)).
(iii)
θk is initialized to −π,π.
(iv)
xk−1, yk−1, and θk−1 are obtained from the solution of the previous CSP (at time k−1).We consider all the state equations from timek−Ξ+1 to time k (where k is the current time and Ξ is the window size): the contractions are performed in a sliding window of length Ξ. In other words, we use Ξ sets of equations (current and past information) in our interval constraint satisfaction problem in order to localize the vehicle. At each time k, we add a new constraint set Ck to the existing constraint sets. If k>Ξ, we delete the oldest constraint set Ck−Ξ from it. The addition (resp. deletion) operation is also known as “restriction” (resp. “relaxation”).The localization DICSP (dynamic ICSP) keeps the same number of constraints (3×Ξ) but changes temporally. The localization DICSP is defined at time k by
(18)DICSPk=ICSPkICSPk−1ICSPk−2⋯ICSPk−Ξ+1.ICP algorithms allow us to solveDICSPk. For our experiment, some local consistency algorithms including HC4 and the strong consistency 3B algorithm have been used.We compared many ICP algorithms for solving our DICSPs. The best results are obtained by the 3B algorithm [35]. Unfortunately, 3B is too slow to work in a real-time environment. And consequently, it cannot be used to solve our real-time problem. We noticed that apart from 3B, no algorithms can contract the θ parameter [36], as explained detailedly in the next section.


## 4.3. Uncorrected Yaw Problem

Vincke and Lambert [34] had shown that classical interval propagation algorithms can not reduce the heading uncertainty of a vehicle equipped with a GPS receiver, a gyro, and odometers. Let us consider a simple forward-backward propagation algorithm, the contraction of the heading uncertainty is performed by (19), which is deduced from (17) in a backward way.
(19)θk−1=θk−1⋂arccosxk−xk−1δsk−δθk2,θk−1=θk−1⋂arcsinyk−yk−1δsk−δθk2.Example 2.
Considering the case study with a straight displacement along thex-axis, the orientation variation is null −δθk/2=0. The correction on θ is realized by arcsinyk−yk−1/δsk and arccosxk−xk−1/δsk.
The width ofyk−1 is around 10 m in most of our experiments: yk−1=−5,5. Thus, we can get
(20)yk−yk−1=−5+ϵy,5−ϵy−−5,5=−10+ϵy,10−ϵy,where ϵy is a small reduction of the imprecision due to a new GPS measurement. δsk is around 1 m:
(21)δsk=1−ϵs,1+ϵs.
Finally,(22)arcsinyk−yk−1δsk=arcsin−10+ϵy,10−ϵy1−ϵs,1+ϵs=−π,π.
The value inside thearcsin function is far beyond −1,1, leading to a full ignorance on the orientation. The same reasoning leads to the same result for the arccos function. Thus, (19) is unable to correct the orientation.During the vehicle’s displacement, the interval representing the heading uncertainty will grow continuously although other parameters of the pose are contracted. It is a big issue because the heading information is important for a good estimation of the vehicle’s position on thex-axis and y-axis. Reference [34] explained the problem but gives no solution. Let us notice that their approach needed to decompose all the constraints of the problem into elementary/binary constraints.The uncorrected yaw problem is observed only with local consistency algorithms which solve constraints one by one. They are subject to the “locality problem”: the solution is always local with regard to one constraint at a time. Only a strong consistency algorithm (3B for instance) can overcome this problem and correct the yaw. Unfortunately, strong consistency algorithms have a huge computing time and are not suited for real-time issue [53]. We propose to solve this problem by introducing a new algorithm which splits the θ parameter and creates multiple DICSP (called νDICSP). The next section details our method.


## 4.4. A Multiple Dynamic Interval Constraint Satisfaction Problem

In order to correct the yaw, we propose to split the variableθk−Ξ into ν variables θk−Ξi,i∈1,2,…,ν where ν stands for the “splitting factor.”Each domainθk−Ξi is then defined by
(23)θk−Ξi=θk−Ξ¯+i−1S¯,θk−Ξ¯+iS¯,where k≥Ξ, S=wθk−Ξ/v is the width of each θk−Ξi, and wθk−Ξ represents the width of the interval θk−Ξ. S¯ and S¯ are, respectively, the round down and the round up of the number S.Example 3.
To illustrate, let us considerθk−Ξ=1,2. If ν=10 is the “splitting factor,” then, the following intervals will be generated: θk−Ξ1=1¯,1.1¯,θk−Ξ2=1.1¯,1.2¯,θk−Ξ3=1.2¯,1.3¯,…,θk−Ξ10=1.9¯,2¯.In the first step of the algorithm,k<Ξ and θk−Ξ is undefined: the splitting is realized on θ0. θk−Ξ is the union of all the θk−Ξi:
(24)θk−Ξ=⋃i=1vθk−Ξi.Other domains remain unchanged (unsplitted). The proposed splitting leads to aνDICSP:
(25)νDICSPk=DICSPk1,DICSPk2,…,DICSPkν,with
(26)DICSPki=ICSPki,ICSPk−1i,…,ICSPk−Ξ+1iT.Algorithm 1: DICPS.
Ensure:DICSPk
(1) (θk−Ξ1,…,θk−Ξv) ← Split(θk−Ξ,v)
(2)fori ← 1 : vdo
(3)DICSPki ← Build(DICSPk,θk−Ξ1)
(4)  [Xki] ← Solve(DICSPki)
(5)end for
(6) [Xres] ← Hull(Xk1,…,Xkv)
(7)return [Xres]The DICSPs (columns of theνDICSP) are separately solved by a local consistency algorithm. The solution of the localization problem is given by applying the interval hull function on the results obtained from the first row of the νDICSPk. It defines the current position and orientation of the vehicle as an interval vector xk,yk,θk. When new measurements are available, the next localization process starts by applying the interval hull function on the results of the last row of the νDICSPk (first step before splitting).Sometimes, one (or more) DICSP does not get a solution: one of the domains is empty. This is an expected behavior that enables fast correction by eliminating a whole set of erroneous pose.The building and the solving of the multiple dynamic interval constraint satisfaction problem are realized by our proposed dynamic interval constraint propagation with the splitting algorithm (DICSP, see Algorithm1), explained hereafter:
(i)
Line 1: the oldest variableθk−Ξ, representing the orientation in the initial DICSP, is split into ν intervals.
(ii)
Line 3: for eachθk−Ξi, a DICSP is generated.
(iii)
Line 4: each DICSP is submitted to a solving process (by an ICP algorithm).
(iv)
Line 6: the new domains, resulting from the process of the preview step, are grouped with an intervalhull function.
(v)
Line 7: the interval hull constitutes an interval vector which is returned as the vehicle position.This algorithm has been implemented and tested on a vehicle running on a test track as explained in the next section.


## 4.5. About Completeness

“The two most appealing features of interval analysis and numerical constraint propagation, when used to solve numerical problems, are completeness and rigor. Completeness means the ability to find all solutions, whereas rigor is the ability to control the rounding errors due to floating-point computation” [54]. The rigor property is kept by our algorithm as we only used interval operators. The completeness is discussed because our algorithm splits the variables before applying classical constraint propagation techniques. We propose a new ICP algorithm with splitting: it splits an interval vector x into boxes x1,x2,…,xm, so that
(27)x=⋃i=1mxi.Then, the range of a functionf· over x, possibly defined by (17), can be evaluated by
(28)fx=f⋃i=1mxi.The bisection usually leads to an improved estimate. Our algorithm takes advantage of this property and combine it with DICSP to solve the uncorrected yaw problem by splitting the parameterθ. According to the interval extension definition given in Section 2.2, we have
(29)fx⊆⋃i=1mfxi.And thanks to the definition of interval hull (see Section2.2), we obtain
(30)fx⊆⨆i=1mfxi.Since⊔i=1mfxi is a genuine superset of the left-hand side, all correct answers are included in it. Furthermore, each term of the interval hull is solved by a local consistency algorithm which ensures that none of the solutions are removed (thus achieving the completeness of our method).


## 5. Results

This section introduces the tools and equipment (vehicle prototype with its sensors) that we used to validate our method as well as the test track where data have been collected. Our algorithm has been compared with 2 interval algorithms and a Bayesian one.


### 5.1. Experimental Setup

The proposed algorithm has been implemented in C language on a computer equipped with an Intel Core i7 CPU 960 @ 3.20 GHz running under Ubuntu 14.04 LTS. Solving has been realized with a set of timestamped data that has been collected on the Satory test track (Figure2(c)) with LIVIC’s prototype (Figures 2(a) and 2(b)) running at an average speed of 50 km/h. A solid-state vertical gyro VG400CC provides the yaw rate data. The global localization is performed by an AgGPS132 (datasheet: http://dragadoshidraulicos.com/batimetria/GPS-Trimble-AGP132.pdf). GPS measurements and gyro/odometer data acquisitions are realized at a 5 Hz frequency. The centimeter reference used for the evaluation of the positioning method is provided by a RTK GPS (datasheet: http://www.ctsystems.eu/Resources/Sagitta.pdf).Figure 2
Experimental platform. (a) CARLLa platform. (b) Embedded hardware architecture. (c) The Satory track with timestamps.
(a)
(b)
(c)


### 5.2. Comparison between DICPS, HC4, and 3B Algorithms

A full lap run last 400 seconds while 2000 localization DICSPs have been solved under different window sizes (Ξ=10,20,30,40,50,100). Figure 3(a) shows how the value of the splitting factor ν influences the correction of heading imprecision in DICPS. The efficiency of the splitting decreases as a hyperbolic function, indicating that the value of the splitting factor ν can be bounded around 20. Nevertheless, the higher both variables are (Ξ and ν), the lower is the localization error.Figure 3
Heading imprecision. (a) Average heading imprecision for different splitting factors. (b) Heading imprecision at each time step for different ICP algorithms.
(a)
(b)Computing time has been measured for HC4, 3B, and our DICPS algorithm. Table2 shows that HC4 is the fastest, but Figure 3(b) indicates its inability to correct the heading imprecision: the heading imprecision increases linearly as the time goes by.Table 2
Mean duration of the algorithm for a localization step.
Algorithms parameters
DICPSΞ=20; ν=20
HC4Ξ=20
3BΞ=20
Duration (ms)
13.32
0.542
5257This is a common result obtained by local consistency algorithms such as HC3 [19], BC3 [21], BC4 [20], and BC5 [22]. DICPS results are observed with three window sizes Ξ (20, 40, and 100) depicted in Figure 3(b). The heading imprecision is stabilized between 10 and 35 degrees depending on the parameters. It shows that the window size is an important parameter that improves the correction of heading imprecision.3B consistency considers the bounds of each variable’s domain. It tends to calculate the solution of the localization DICSP for each bound (see Section3.2). If no solution is found, the bound is eliminated from the considered domain and so on. We use 3B with the same window size than DICPS: Ξ=20. The observed results are the same while 3B computing time is much larger than the DICPS one: 5257 ms for 3B versus 13.32 ms for DICPS (see Table 2). Different window sizes give the same localization results but with a 3B prohibitive computing time.If the window sizeΞ is increased to 40 while keeping the same splitting (ν=20), then the computing time of DICPS is 172 ms. Thus, it is still real-time while ensuring a 16° (±8°) imprecision in average. This imprecision is below the value provided by other interval approaches (including CP and BESE [55]) in the same environment.


### 5.3. Comparison between DICPS and EKF

The previous section shows that DICPS outdoes the other methods based on interval analysis. This section aims to compare DICPS with a wide-spread Bayesian filter: the extended Kalman filter (EKF).


#### 5.3.1. Theoretical Comparison

DICPS considers bounded errors (with no assumption on the error inside the bound) while EKF deals with Gaussian errors. DICPS returns interval vectors which contain the vehicle’s pose (it searches all solutions that are consistent with the model and data of the given problem). EKF gives a vector (that has the highest probability to be the solution) and its associated Gaussian uncertainty, represented by a matrix.


#### 5.3.2. Experimental Comparison

We set up DICPS with parametersν=20 and Ξ=40. In order to ensure fairness, EKF and DICPS use the same data set.The EKF is fed with the standard deviationσ of the sensors whereas DICPS uses a 3σ error bound which corresponds to a 0.99 confidence level. Consequently, where we should consider finding a data with a 0.99 probability, the DICPS assumes to find it with a 1 probability.Figure4(a) shows the interval error of the DICPS and the EKF. The interval error is defined by the upper and lower bounds of the estimated state minus the corresponding bound reference state. A filter exhibits good results if its interval error is thin and always includes the reference value zero. The imprecision area of DICPS (using a 3σ error bound) has directly been used to compute the interval error. For the EKF, the imprecision (standard deviation) has been magnified 3 times.Figure 4
Comparison between DICPS and EKF. (a) DICPS (3σ) and inconsistent EKF with 3σ error bounds. (b) DICPS (3σ) and consistent EKF with 17σ error bounds.
(a)
(b)The EKF is more confident (has a thinner corridor) than DICPS, but the EKF is sometimes inconsistent (does not include the zero value) whereas DICPS is always consistent. The large peaks (aroundt = 100, 150, 200, 340, and 370) are caused by GPS measurements with large imprecision. The top peaks define steps when the imprecision of the robot state has increased before being reduced thanks to GPS measurement. The great inconsistency of the EKF around t=200 s is due to a large number of repeated biased GPS measurements. The EKF greatly underestimates its covariance matrices when dealing with repeated biased measurements. DICPS does not suffer from those biased measurements because it has no assumption on the distribution of the noise (contrarily to the EKF).We could get a consistent EKF by tuning carefully (enlarging) the noise value for each simulation (we can do it a posteriori by using the reference). However, such an EKF tuning does not seem feasible for the localization problem in the real world because we do not have such a reference (if we have such a reference then computing the localization will be useless). Nevertheless, as part of this paper, we have such a reference and we have computed thex value for the EKF to be consistent. We found 17 and drew the 17σ interval error in Figure 4(b). Thus, we can compare both consistent filters. The resulting EKF corridors are larger than those of the consistent DICPS. Consequently, even if it was possible to compute the x value during the localization, the EKF results would not be better than the results of DICPS.


## 5.1. Experimental Setup

The proposed algorithm has been implemented in C language on a computer equipped with an Intel Core i7 CPU 960 @ 3.20 GHz running under Ubuntu 14.04 LTS. Solving has been realized with a set of timestamped data that has been collected on the Satory test track (Figure2(c)) with LIVIC’s prototype (Figures 2(a) and 2(b)) running at an average speed of 50 km/h. A solid-state vertical gyro VG400CC provides the yaw rate data. The global localization is performed by an AgGPS132 (datasheet: http://dragadoshidraulicos.com/batimetria/GPS-Trimble-AGP132.pdf). GPS measurements and gyro/odometer data acquisitions are realized at a 5 Hz frequency. The centimeter reference used for the evaluation of the positioning method is provided by a RTK GPS (datasheet: http://www.ctsystems.eu/Resources/Sagitta.pdf).Figure 2
Experimental platform. (a) CARLLa platform. (b) Embedded hardware architecture. (c) The Satory track with timestamps.
(a)
(b)
(c)


## 5.2. Comparison between DICPS, HC4, and 3B Algorithms

A full lap run last 400 seconds while 2000 localization DICSPs have been solved under different window sizes (Ξ=10,20,30,40,50,100). Figure 3(a) shows how the value of the splitting factor ν influences the correction of heading imprecision in DICPS. The efficiency of the splitting decreases as a hyperbolic function, indicating that the value of the splitting factor ν can be bounded around 20. Nevertheless, the higher both variables are (Ξ and ν), the lower is the localization error.Figure 3
Heading imprecision. (a) Average heading imprecision for different splitting factors. (b) Heading imprecision at each time step for different ICP algorithms.
(a)
(b)Computing time has been measured for HC4, 3B, and our DICPS algorithm. Table2 shows that HC4 is the fastest, but Figure 3(b) indicates its inability to correct the heading imprecision: the heading imprecision increases linearly as the time goes by.Table 2
Mean duration of the algorithm for a localization step.
Algorithms parameters
DICPSΞ=20; ν=20
HC4Ξ=20
3BΞ=20
Duration (ms)
13.32
0.542
5257This is a common result obtained by local consistency algorithms such as HC3 [19], BC3 [21], BC4 [20], and BC5 [22]. DICPS results are observed with three window sizes Ξ (20, 40, and 100) depicted in Figure 3(b). The heading imprecision is stabilized between 10 and 35 degrees depending on the parameters. It shows that the window size is an important parameter that improves the correction of heading imprecision.3B consistency considers the bounds of each variable’s domain. It tends to calculate the solution of the localization DICSP for each bound (see Section3.2). If no solution is found, the bound is eliminated from the considered domain and so on. We use 3B with the same window size than DICPS: Ξ=20. The observed results are the same while 3B computing time is much larger than the DICPS one: 5257 ms for 3B versus 13.32 ms for DICPS (see Table 2). Different window sizes give the same localization results but with a 3B prohibitive computing time.If the window sizeΞ is increased to 40 while keeping the same splitting (ν=20), then the computing time of DICPS is 172 ms. Thus, it is still real-time while ensuring a 16° (±8°) imprecision in average. This imprecision is below the value provided by other interval approaches (including CP and BESE [55]) in the same environment.


## 5.3. Comparison between DICPS and EKF

The previous section shows that DICPS outdoes the other methods based on interval analysis. This section aims to compare DICPS with a wide-spread Bayesian filter: the extended Kalman filter (EKF).


### 5.3.1. Theoretical Comparison

DICPS considers bounded errors (with no assumption on the error inside the bound) while EKF deals with Gaussian errors. DICPS returns interval vectors which contain the vehicle’s pose (it searches all solutions that are consistent with the model and data of the given problem). EKF gives a vector (that has the highest probability to be the solution) and its associated Gaussian uncertainty, represented by a matrix.


### 5.3.2. Experimental Comparison

We set up DICPS with parametersν=20 and Ξ=40. In order to ensure fairness, EKF and DICPS use the same data set.The EKF is fed with the standard deviationσ of the sensors whereas DICPS uses a 3σ error bound which corresponds to a 0.99 confidence level. Consequently, where we should consider finding a data with a 0.99 probability, the DICPS assumes to find it with a 1 probability.Figure4(a) shows the interval error of the DICPS and the EKF. The interval error is defined by the upper and lower bounds of the estimated state minus the corresponding bound reference state. A filter exhibits good results if its interval error is thin and always includes the reference value zero. The imprecision area of DICPS (using a 3σ error bound) has directly been used to compute the interval error. For the EKF, the imprecision (standard deviation) has been magnified 3 times.Figure 4
Comparison between DICPS and EKF. (a) DICPS (3σ) and inconsistent EKF with 3σ error bounds. (b) DICPS (3σ) and consistent EKF with 17σ error bounds.
(a)
(b)The EKF is more confident (has a thinner corridor) than DICPS, but the EKF is sometimes inconsistent (does not include the zero value) whereas DICPS is always consistent. The large peaks (aroundt = 100, 150, 200, 340, and 370) are caused by GPS measurements with large imprecision. The top peaks define steps when the imprecision of the robot state has increased before being reduced thanks to GPS measurement. The great inconsistency of the EKF around t=200 s is due to a large number of repeated biased GPS measurements. The EKF greatly underestimates its covariance matrices when dealing with repeated biased measurements. DICPS does not suffer from those biased measurements because it has no assumption on the distribution of the noise (contrarily to the EKF).We could get a consistent EKF by tuning carefully (enlarging) the noise value for each simulation (we can do it a posteriori by using the reference). However, such an EKF tuning does not seem feasible for the localization problem in the real world because we do not have such a reference (if we have such a reference then computing the localization will be useless). Nevertheless, as part of this paper, we have such a reference and we have computed thex value for the EKF to be consistent. We found 17 and drew the 17σ interval error in Figure 4(b). Thus, we can compare both consistent filters. The resulting EKF corridors are larger than those of the consistent DICPS. Consequently, even if it was possible to compute the x value during the localization, the EKF results would not be better than the results of DICPS.


## 5.3.1. Theoretical Comparison

DICPS considers bounded errors (with no assumption on the error inside the bound) while EKF deals with Gaussian errors. DICPS returns interval vectors which contain the vehicle’s pose (it searches all solutions that are consistent with the model and data of the given problem). EKF gives a vector (that has the highest probability to be the solution) and its associated Gaussian uncertainty, represented by a matrix.


## 5.3.2. Experimental Comparison

We set up DICPS with parametersν=20 and Ξ=40. In order to ensure fairness, EKF and DICPS use the same data set.The EKF is fed with the standard deviationσ of the sensors whereas DICPS uses a 3σ error bound which corresponds to a 0.99 confidence level. Consequently, where we should consider finding a data with a 0.99 probability, the DICPS assumes to find it with a 1 probability.Figure4(a) shows the interval error of the DICPS and the EKF. The interval error is defined by the upper and lower bounds of the estimated state minus the corresponding bound reference state. A filter exhibits good results if its interval error is thin and always includes the reference value zero. The imprecision area of DICPS (using a 3σ error bound) has directly been used to compute the interval error. For the EKF, the imprecision (standard deviation) has been magnified 3 times.Figure 4
Comparison between DICPS and EKF. (a) DICPS (3σ) and inconsistent EKF with 3σ error bounds. (b) DICPS (3σ) and consistent EKF with 17σ error bounds.
(a)
(b)The EKF is more confident (has a thinner corridor) than DICPS, but the EKF is sometimes inconsistent (does not include the zero value) whereas DICPS is always consistent. The large peaks (aroundt = 100, 150, 200, 340, and 370) are caused by GPS measurements with large imprecision. The top peaks define steps when the imprecision of the robot state has increased before being reduced thanks to GPS measurement. The great inconsistency of the EKF around t=200 s is due to a large number of repeated biased GPS measurements. The EKF greatly underestimates its covariance matrices when dealing with repeated biased measurements. DICPS does not suffer from those biased measurements because it has no assumption on the distribution of the noise (contrarily to the EKF).We could get a consistent EKF by tuning carefully (enlarging) the noise value for each simulation (we can do it a posteriori by using the reference). However, such an EKF tuning does not seem feasible for the localization problem in the real world because we do not have such a reference (if we have such a reference then computing the localization will be useless). Nevertheless, as part of this paper, we have such a reference and we have computed thex value for the EKF to be consistent. We found 17 and drew the 17σ interval error in Figure 4(b). Thus, we can compare both consistent filters. The resulting EKF corridors are larger than those of the consistent DICPS. Consequently, even if it was possible to compute the x value during the localization, the EKF results would not be better than the results of DICPS.


## 6. Conclusion

This paper deals with the vehicle localization problem using interval analysis. Different algorithms have been compared on a vehicle equipped with a GPS receiver, a gyro, and odometers. Every algorithm can correct the position. But under real-time constraints, no ICP algorithms were able to correct the vehicle’s orientation. Only 3B was capable of orientation correction but at the cost of a huge computing time.Our algorithm (DICPS) is able to correct the orientation and is real-time executable. As the computation time increases with the splitting factor, we recommend to conduct DICPS with parametersΞ=20 and ν=40 in order to keep a reasonable calculation time (172 ms for each localization step). DICPS has been shown to reduce the orientation imprecision around ±8. This method is robust and provides consistent results.DICPS has been compared to the well-known extended Kalman filter. We point out that the EKF can locally converge towards a wrong solution due to bias measurements which lead to a huge local inconsistency. We claim that consistency is the biggest advantage of DICPS over Bayesian approaches.Future works will deal with improvements to further reduce the imprecision and minimizing the CPU load. It can be achieved by dynamically defining the involved parameters (window size and splitting factor). In addition, no change or partial change on variables will allow us to simplify the localization problem by removing constraints. Finally, small changes could be stacked up, waiting for a significant amount of change to start the constraint propagation process.



---

*Source: 3769058-2018-04-10.xml*