Corentin Sanchez

Philippe Xu

Philippe Bonnifait

Alexandre Armand

Integrity Management of the Reachable Space With Lane Grid Maps

come   L'archive ouverte pluridisciplinaire

Integrity Management of the Reachable Space with

Lane Grid Maps Corentin Sanchez*, Philippe Xu*, Philippe Bonnifait*, Alexandre Armand** Abstract-For an autonomous vehicle, reliable situation understanding is a key component of safe navigation. An incorrect prediction of an upcoming situation means that erroneous information may be supplied to the decision-making process, leading to hazardous outcomes. It is therefore of great importance to estimate the driving areas that are reachable by other interacting road users, without introducing misleading information. This paper presents a means of handling the integrity of prediction information, given the imperfection of object prediction, via a Lane Grid Map, that is to say a spatial representation of the situation at a tactical level, based on the topological layer of a high-definition map. We demonstrate experimentally, using real data, how the spatial sampling step of the grid representation can be used to manage the integrity of prediction information. Moreover, addressing interactions during the prediction makes it possible to handle some particular situations safely. We show how some interactions can be utilized via the concept of neutralization. To quantify the integrity of the prediction, we propose the use of two metrics, namely False Negative Rate and Neutralized Time Interval. Experiments were carried out with three vehicles to evaluate the integrity of the prediction using these metrics.

Index Terms-Situation understanding, prediction of driving situations, integrity of navigation information

I. INTRODUCTION

O NCE an autonomous vehicle navigating on open roads has its own representation of the current situation, it needs to obtain a prediction of what is going to happen subsequently, which is the highest level of situation understanding. Usually, prediction is concerned with the most likely situation that the vehicle is about to encounter, and is part of a situation awareness strategy [START_REF] Endsley | Toward a Theory of Situation Awareness in Dynamic Systems[END_REF]. Prediction enables actions to be planned via decision-making processes. A large number of works have been undertaken in this area. Decision planning and prediction can be done at different levels of abstraction [START_REF] Vendrell | Robot planning and replanning using decomposition, abstraction, deduction, and prediction[END_REF]. In the present work we focus on prediction over a period of a few seconds at the maneuver level, also known as the tactical level. Prediction must be done without compromising safety, as the likelihood of unexpected behaviors increases as time goes by.

A key issue in prediction is providing reliable information over the relevant time horizon. A misunderstanding of the situation can result in behaviors that are hazardous. Preventing misunderstandings of this kind depends partly on ensuring that the navigation information provided to the decision and control systems is not misleading, and a considerable amount of work in this area is to be found in the literature [START_REF] Miller | Tightly-coupled GPS / INS system design for autonomous urban navigation[END_REF] [START_REF] Hage | Localization Integrity for Intelligent Vehicles Through Fault Detection and Position Error Characterization[END_REF]. The key Fig. 1: At the current time t 0 , the ego vehicle has a grid-based representation of the situation with free (green) and occupied (red) spaces (a). To turn right, it needs to predict the evolution of the situation at a future time t 0 + ∆t. In situation (b), the prediction is misleading as the true position of the yellow vehicle is outside of the bounds of the reachable orange area. concept here is integrity, which has been extensively studied for GNSS systems [START_REF] Zhu | GNSS Position Integrity in Urban Environments: A Review of Literature[END_REF], and is a concept that has also been applied to certain other problems including lane-level mapmatching [START_REF] Li | Map-aided dead-reckoning with lane-level maps and integrity monitoring[END_REF]. In this paper, the core of our methodological approach involves extending this concept of integrity to the information encoded in a grid-based representation. To set the scene, let us consider an example of misleading information. Fig. 1.a shows a situation from the point of view of the blue vehicle at the current time t 0 . This representation has been produced by the blue vehicle itself using its own exteroceptive sensors, a localization system and an High-Definition (HD) map. The true position of the observed yellow vehicle is contained within the occupied cells in red. A prediction module computes the reachable bounds of the yellow vehicle and the corresponding reachable cells at some future time t 0 + ∆t. In Fig. 1b, the predicted bounds are incorrect, since the true future position of the yellow vehicle is no longer fully within the bounds. The incorrect bounds prediction may have a variety of causes, and in this particular case it may be caused by an underestimation of speed. Here we have an incorrect prediction that could potentially lead to a hazardous situation, since the ego vehicle may consider wrongly that it has time to turn right in front of the yellow vehicle.

Our methodology adheres to the outline shown in Fig. 2. We use the formalism of Lane Grid Maps (LGMs), introduced in [START_REF] Sanchez | Lane level context and hidden space characterization for autonomous driving[END_REF]. A geometric model of this kind is well suited to formal safe decision methods such as that presented in [START_REF] Shalev-Shwartz | On a Formal Model of Safe and Scalable Self-driving Cars[END_REF]. In addition, the integrity of the information it contains can be managed, and consequently the decision and control methods of the vehicle are not at risk of being misled.

HD map

Localization

Sampling step Prediction model Perception

Lanes of interest

Space discretization

State characterization

State prediction

Decision making

LGM skeleton

LGM(t 0 )

LGM(t 0 +Δt|t 0 ) LGM backbone Fig. 2: LGM computation and prediction diagram. Grey boxes are inputs. The white boxes are the different steps to build the LGM and its prediction. The sampling step, which is a key parameter of the LGM integrity, is depicted in black.

The methodological contribution of our paper is threefold. First, the concept of integrity is formalized within a gridbased representation of the driving situation. More specifically, we introduce integrity metrics in terms of spatial occupancy and show that this integrity can be managed by tuning a spatial sampling parameter. Second, we extend the concept of integrity to situation prediction, where we quantify integrity in terms of spatial reachability. Third, the representation is enriched by taking into account physical interactions among road users, and we show that the same sampling step tuning process still satisfies the integrity constraint. All the methodological contributions were validated empirically via real experiments in the city of Compiègne, France.

The paper is organized as follows. Section II is devoted to related work, and we present different lane-level spatial representations and prediction methods that have been used in the literature. The three contributions listed in the previous paragraph are then detailed in three distinct sections, as follows. First, section III presents the LGM representation covering the computation of the lanes of interest, the discretization process and the characterization of cells. The integrity of the LGM information is formalized. Experimental results are presented to show how LGM integrity can be managed using the sampling step parameter. Second, in section IV, we extend the integrity concept to the reachability analysis of predicted LGMs. Third, section V proposes an enrichment of situation understanding via the concept of "Lane Neutralization", induced by physical interactions among road users. Experimental results are presented within the different sections.

II. RELATED WORK A. World modelling

The literature features various architectures that seek to set up a so-called world or context model [START_REF] Tas | Functional system architectures towards fully automated driving[END_REF]- [START_REF] Ulbrich | Towards a Functional System Architecture for Automated Vehicles[END_REF]. In [START_REF] Ulbrich | Towards a Functional System Architecture for Automated Vehicles[END_REF], a distinction is made between scene representation from context modelling and situation representation from a planning point of view (defined earlier in [START_REF] Ulbrich | Defining and Substantiating the Terms Scene, Situation, and Scenario for Automated Driving[END_REF]). In [START_REF] Tas | Functional system architectures towards fully automated driving[END_REF], the environment model, together with the situation analysis and prediction are instead part of the perception module.

Given a module that stores a world model, a situation can have several abstraction levels in terms of information and comprehension mechanisms. The three levels of abstraction identified some years ago in [START_REF] Michon | A Critical View of Driver Behavior Models: What Do We Know, What Should We Do?[END_REF] are those also used in [START_REF] Ulbrich | Towards a Functional System Architecture for Automated Vehicles[END_REF]. Among these, the Operational Level is the lowest level, offering a fine resolution of the space that provides a fine metric representation of the vehicle surroundings, and this can be used for short-term prediction. The Tactical Level is the intermediate level. It corresponds to a lane level, and the prediction of a situation at this level is on a longer time horizon (e.g. maneuvers). Finally, the highest level is the Strategic Level, having the most abstracted view of information. The Strategic Level offers a resolution of several hundred meters, and provides what is essentially a symbolic representation of the environment. At this level the mission planner, for example, plans a global trajectory over a wide space and time horizon.

The present research focuses on the tactical level, i.e. the lane level: the vehicle is aware of topological map information, and prediction at this level incorporates interactions. Two types of information can be distinguished, via object and spatial representations respectively. However, there is a need to combine these two different types of representation, as highlighted in the architectures presented in [START_REF] Tas | Functional system architectures towards fully automated driving[END_REF] and [START_REF] Ulbrich | Graph-based context representation, environment modeling and information aggregation for automated driving[END_REF].

B. Spatial representations

Occupancy grid maps are probably the most popular discretized representation of environments, not least because they can be used for localization, planning, and tracking, as well as navigation [START_REF] Heide | Performance Optimization of Autonomous Platforms in Unstructured Outdoor Environments Using a Novel Constrained Planning Approach[END_REF], [START_REF] Moras | Credibilist occupancy grids for vehicle perception in dynamic environments[END_REF]. In general, the state of cells is defined by a distribution over the possible state values. Most of the time, the state of a cell is either free or occupied [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF]. For automotive applications, cells can be characterized as drivable and not drivable as proposed in [START_REF] Michalke | The narrow road assistant -evolution towards highly automated driving in inner city[END_REF], or by risk metrics as in [START_REF] Laconte | Dynamic lambda-field: A counterpart of the bayesian occupancy grid for risk assessment in dynamic environments[END_REF].

Parametric free space maps [START_REF] Schreier | Compact Representation of Dynamic Driving Environments for ADAS by Parametric Free Space and Dynamic Object Maps[END_REF], created by combining parametric curves and geometric primitives, have been proposed to encompass the free drivable space. Uncertainties on the system inputs can be taken into account, giving rise to probability distributions corresponding to different parameters. The advantage of this approach is the compactness of the generated data. However, this approach, since it does not explicitly deal with objects, makes prediction operations more difficult.

C. Set-based prediction

There are several survey articles on motion prediction, including, for example, [START_REF] Lefèvre | A survey on motion prediction and risk assessment for intelligent vehicles[END_REF] and [START_REF] Rudenko | Human motion trajectory prediction: A survey[END_REF]. Prediction methods may be separated into different categories relating to the operational or tactical levels. These methods may or may not take into consideration interactions at the strategic level.

A spatial representation that is able to handle occlusions and objects is needed for tasks like decision-making, but also for risk analysis [START_REF] Yu | Occlusion-Aware Risk Assessment for Autonomous Driving in Urban Environments[END_REF] and safety evaluation [START_REF] Hoermann | Entering crossroads with blind corners. A safe strategy for autonomous vehicles[END_REF]. In [START_REF] Orzechowski | Tackling Occlusions & Limited Sensor Range with Set-based Safety Verification[END_REF], setbased prediction is used in predicting the path and intention of a hypothetical road user that may potentially be hidden in an occluded area. Particle filters have also been used in determining the plausibility of a hidden road user [START_REF] Narksri | Crossing Blind Intersections from a Full Stop Using Estimated Visibility of Approaching Vehicles[END_REF].

Set-based prediction is also presented in [START_REF] Koschi | SPOT: A tool for set-based prediction of traffic participants[END_REF] in the context of the SPOT project, where reachable areas of independent vehicles, known as reachable sets, are computed. For a given time horizon, the space that may be occupied by each vehicle during this interval is computed. In order to take into account interactions between road users, an intersection of different reachable sets can be performed by adding constraints, thus refining the reachability analysis strategy. Reachable space is bounded by the road layout as indicated by topological information, and computations are subject to some constraints [START_REF]Interaction-aware occupancy prediction of road vehicles[END_REF]. For instance, a negative speed is not allowed, since vehicles always go forwards. When a vehicle cannot overtake a vehicle on a single lane, its prediction must remain consistent. There are several articles based on reachable sets that have given rise to further work, e.g. in trajectory planning [START_REF] Manzinger | Using Reachable Sets for Trajectory Planning of Automated Vehicles[END_REF].

D. Positioning of the approach

In this paper we propose a world model based on a discrete representation of the navigable space guided by an HD map. This approach seeks to leverage the advantages of existing approaches while providing integrity control mechanisms at a tactical level. Integrity is addressed both in real time and in predicting reachable space, where some physical constraints among road users are taken into account. Our approach makes use of an LGM representation, as described in the next section.

III. LANE GRID MAP: A LANE-LEVEL REPRESENTATION

An LGM is an ego-centered spatial representation at a tactical level. It allows situation understanding in the areas of interest of the ego vehicle. The LGM focuses attention on areas whose relevance will be determined by the particular road network. A localization system is used to map-match the pose of the vehicle to a lane on a road. An Interaction Graph (IG) structures the areas of interest using an HD map [START_REF] Sanchez | Lane level context and hidden space characterization for autonomous driving[END_REF]. The map is a graph structure with topological and metrical levels. The lanes in the map are represented by their center as a polyline with a high accuracy, and are connected through spatial relations, e.g. crossing or merging or adjacency. Additional information on the road infrastructure may also be available, such as lane width, traffic law priorities, and speed limits. Each node of the graph corresponds to a specific area. Fig. 3 shows the application of the IG representation. The lanes that the ego vehicle intends to follow are shown in blue. There are nodes corresponding to areas that are of high importance, called primary-order nodes, e.g. the red lanes that cross and merge with the blue lane, and others that are of secondary order, e.g. the orange lanes that have direct interactions with the red lane. In Fig. 3, only indirect interactions with respect to the merging lane in red are shown.

The ego vehicle must focus on the space where it is likely to evolve and on the scene elements with which it may interact. The LGM lane-level representation is built upon the IG that focuses on areas of interest and serves as a backbone.

A. A lane-level space representation

At the lane level, the autonomous vehicle has to convert the topological information provided by the IG into a metrical representation. The proposed lane-level grid representation is a spatial grid based on a discretization of the lanes of interest along their longitudinal direction. Based on the geometric layer of an HD map, the lanes are discretized in successive contiguous quadrangles, i.e. cells, that cover the space of the lanes in a continuous way. An LGM has two parameters to be set: a cross-track cell width and an along-track cell length.

Fig. 4 shows an LGM representation of a merging intersection situation alongside a classical Cartesian Occupancy Grid (OG). The cells of the LGM are only located in areas of interest of the ego vehicle. Compared to an OG, an LGM is more compact, scalable and easier to manipulate, since information is encoded such that it fits the lane-level context that the ego vehicle encounters.

As shown in Fig. 4, the same area can be characterized differently by cells at junctions because of overlapping navigation corridors. This highlights that the cells are contextualized by the lanes they belong to. In the rest of this paper, we use only a simplified representation of the LGM, where the discretization is applied only in the along-track direction. This simplification means that each cell encompasses the entire track width in the lateral direction. The LGM is therefore defined by a set of cells LGM = {c i |∀i ∈ [0; N ]} where each cell c i is represented as a bounded interval of curvilinear abscissa [c i ; c i ] along the lanes.

B. Occupancy characterization

Once the skeleton of an LGM has been constructed using the IG, the objective is to establish the spatial occupancy of each cell on the basis of information provided by a perception module, using a 3D Lidar for instance [START_REF] Li | Lidar for autonomous driving: The principles, challenges, and trends for automotive lidar and perception systems[END_REF].

Fig. 5 is an illustration of the characterization process using an LGM. In this work, we assume that a perception module provides the free space characterized as a polygon [START_REF] Schreier | Compact Representation of Dynamic Driving Environments for ADAS by Parametric Free Space and Dynamic Object Maps[END_REF] and a list of detected road users also represented by polygons. In Fig. 5 the perception module detects two road users, depicted by the two red objects. It also computes the free space within the range of the sensor. A cell in the LGM is considered to be occupied if it intersects with an object, and it is considered free if it is fully included within the free space. Otherwise the cell is designated as unknown. [START_REF] Sanchez | Lane level context and hidden space characterization for autonomous driving[END_REF].

C. LGM integrity

With an LGM representation, integrity is the ability to supply non-misleading information to decision-making systems. For example, characterizing a driving space as free when it is actually occupied may lead to a collision. The grid's sampling step parameter determines the level of detail to be used for the lane. The larger the cells, the coarser the information, calling for more cautious decision-making. Conversely, oversampling can lead to a loss of integrity resulting from the imperfect input coming from the perception system, which may give rise to decisions that are hazardous.

1) Experimental methodology for evaluating LGM integrity:

To evaluate the integrity level of a given LGM we previously developed an experimental methodology in [START_REF] Sanchez | Spatial Sampling and Integrity in Lane Grid Maps[END_REF]. The integrity metric on which this rests is presented below. Our methodology involves carrying out experiments with several vehicles equipped with a localization ground truth. Fig. 6 shows three vehicles that were used to carry out the experiments. The blue car is the ego vehicle whose LGM is being evaluated. This vehicle is equipped with a Lidar sensor (a Velodyne VLP-32C) with a 360 • field of view, a theoretical range of 100 meters, and running at 10 Hz. An HD map of the city of Compiègne was used as prior information to build the IG and the LGM.

During the evaluation experiments we ensured that only the gray truck and white car shown in Fig. 6 were in the lanes of interest of the blue vehicle. This allowed us to acquire datasets without other road users for which we did not have a ground truth.

2)

LGM ground truth generation: Post-processed GNSS PPK corrections were used to have a centimeter-level localization of each vehicle at a rate of 50 Hz. To get an LGM ground truth at each time instant, the characterization process was performed using the ground truth position of the three vehicles recorded in the dataset. In other words, this was done without using the perception system. Given the accurate pose of the vehicles on the road and their dimensions, their occupancy was approximated by 2D bounding rectangles. Any cell overlapped by these rectangles was considered occupied, and the remaining cells were considered free. The cells of the ground truth LGM are all therefore either Free (F ) or Occupied (O).

Cell i+1 Cell i O F U O O O O F O F U U O U U Ground truth
LGM

LGM

LGM ×2 ÷2 X Fig. 8: The cells are aggregated two by two then separated back for evaluation. The false negative cell marked with a cross disappears after cells aggregation.

3) Integrity metric: Combining the different configurations, for each cell of the LGM there are six possible indicators obtained from the combination of the two characterizations as defined in Table I. The correspondence is shown in Fig. 7.

Two indicators are of interest. F P corresponds to free cells misclassified as occupied. This is an overcautious indicator that does not induce a safety problem. F N is the most important indicator in terms of integrity, as it corresponds to occupied cells misclassified as free. It is a misleading indicator that should occur as little as possible. If the space around the vehicle is considered free whereas in reality it is occupied, this may lead to a hazardous decision-making. The additional indicators U P and U N are not considered misleading, since here the cell state is unknown and consequently does not provide any information (free or occupied) that potentially may be misleading.

To evaluate the integrity, we therefore compute the frequency of occurrence of the F N indicator. This measure considers only cells that have been characterized as free or occupied, i.e. missing information is excluded. The False Negative Rate (F N R), also called the "miss rate", is the frequency of F N over all occupied cells, and is given by:

F N R = F N F N +T P (1)
In the remainder of the paper we seek to build a mechanism to maintain the F N R below a given threshold termed the Target Integrity Risk (TIR), which is set beforehand in line with a specific application's safety requirement.

4) Sampling strategy: We show in this study that the sampling step is able to control the integrity level of the information provided by an LGM, according to a given operational design domain. One of LGM's major contributions is therefore its ability to manage the integrity of the information, thanks to its capacity to control the F N R via the sampling step.

In order to tune the sampling step, we propose a methodology as shown in Fig. 8. Typically the step is chosen to be the one of the ground truth. To increase the sampling step, the cells can be aggregated. For instance, to have a sampling step twice as large, the cells are combined two by two using the aggregation rule presented in Table II. Then, to perform the comparison with the ground truth, the aggregated cells are separated back into the initial sampling step, as shown by the last row in Fig. 8. We can see that the case of F N has been replaced by a case of T P , which is no longer misleading.

It should be noted that the aggregation rule can also be used in real time to obtain an LGM with a desired sampling step, depending on an application's specific needs with respect to situation understanding.

5)

Results: Here we report experimental results showing that it is possible to set a sampling step for a given TIR. A dataset containing sensor data was recorded using three vehicles in real road conditions. The ego vehicle was following the two others along a 2.1km trajectory. The polygons of the perceived vehicles were used to compute the LGM results [START_REF] Bernardi | High Integrity Lane-level Occupancy Estimation of Road Obstacles Through LiDAR and HD Map Data Fusion[END_REF]. A simple geometric ground fitting based on the Principal Component Analysis algorithm and a clustering algorithm was used on point-cloud data to extract object clusters surrounding the ego vehicle [START_REF] Zermas | Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications[END_REF]. The geometric layer of the HD map was used to retain only clusters that were completely or partially on the road. At each Lidar scan, a free space polygon was generated over a radial grid using the shortest distance between the points of the clusters. The clusters were then tracked in order to compute estimates of their heading and speed, in order to be able to represent road users as objects in the form of polygons. To simulate different levels of imprecision in the detection of the road users, we added random Gaussian noise with different standard deviations to the detected polygons. Fig. 9 shows that F N R falls as the sampling step increases. As the number of cells classified as occupied grows, these occupied cells cover a larger area, encompassing nearby cells that were incorrectly classified as free because of the localization error. Conversely, oversampling the LGM produces more misleading information, since F N R increases as the sampling step gets closer to zero.

As the standard deviation of the noise increases, F N R decreases, but at a higher level. This shows that it is possible to control F N R under a given integrity risk by increasing the sampling step. For example, if a maximum integrity risk of 0.3 %, i.e. typically the risk associated with a 3σ Gaussian confidence interval, is specified for F N R, the integrity requirement can be met by setting the sampling step to the value shown by the red dots in Fig. 9.a. To compute these red dots, the different curves were interpolated in a logarithmic scale (dashed curves) and intersected with the required TIR. For a localization error with a 0.5 m standard deviation, a sampling step of 3.0 m is needed, while for a standard deviation smaller than 0.2, a sampling step lower than 1 m is enough. In other words, this graph shows that if the system is not able to properly model the localization uncertainty, a correctly chosen sampling stem can ensure the level of integrity as defined by the functional domain. For a given TIR, the greater the uncertainty, the larger the sampling step.

Note that even without adding noise (red curve), the minimal sampling step needs to be increased to at least 20 cm, in order to cover various unmodeled residual errors from the perception and the localization ground truth. IV. LGM PREDICTION Once the autonomous vehicle has built an LGM at a given time t 0 using its own sensors, the next step is to predict the evolution of this representation over time. We use the notation LGM (t|t 0 ) for any time t in a time horizon [t 0 ; t 0 + ∆t]. Prediction allows the decision-making module to plan actions over a given time horizon.

In order to avoid dangerous decisions, prediction needs to remain consistent without introducing misleading information. This section shows how the same spatial sampling step strategy can be extended such that a given required integrity level can be reached, regardless of the accuracy of a given object-level prediction module.

Reachable sets are easy to represent in a predicted LGM, since they can be seen as a list of polygons. As a consequence, the same characterization process can be applied. The higher the sampling rate, the finer the representation. Reachable sets can therefore be seen as a finer representation of an LGM. Where the cross-track and along-track sampling steps of an LGM tend to zero, this representation tends to be continuous and thus leads to the classical the reachable sets formalism. As sampling steps increase in size, outer approximations are made and the discrete LGM representation encompasses the continuous case.

A. Prediction characterization

In the static LGM presented in the previous section, the characterization space was separated into three classes, namely: Free, Occupied and Unknown. In the prediction space, on the other hand, the two relevant classes are Reachable and Non-Reachable. A cell characterized as Reachable at a given time t means that a road user could potentially reach the cell at that time t. In particular, a cell with an Unknown state will be considered as Reachable because a potential hidden road user could be located in the cell. Conversely, a cell characterized as Non-Reachable means that no road user, visible or hidden, can reach it, which is equivalent to the Free state. In the rest of the paper, Free and Non-Reachable are considered equivalent.

B. Reachability for prediction

The prediction stage first initializes every cell in the predicted LGM (t|t 0 ) as free (F ). The reachability of road users and hidden space is then addressed.

1) Road users: In this paper we are not interested in building a novel object predictor, but we take as input a given module and integrate it into the LGM prediction framework. Therefore, for a given road user, we assume that an object predictor first computes the bounds along the center line of the road lane as an along-track curvilinear abscissa interval

s(t) = [s(t); s(t)] , (2)
which can be seen as a simplified one-dimensional reachable set.

The lateral prediction of a road user is handled along its taken path. It is assumed that a road user prediction may extend along several paths, as long as these paths belong to the LGM. The ability of a vehicle to drive on a path outside the LGM has no relevance for the ego vehicle, since being outside the LGM implies that there is no further interaction with this vehicle.

2) Hidden space: The same principles are applied in handling the hidden space. The hidden space may contain road users. If there are hidden vehicles, their lengths are unknown. Therefore, in the worst case, every cell may contain a potential road user whose speed is unknown. Equivalently, a single virtual road user can be considered, aggregating several contiguous hidden cells such that the interval s(t 0 ) corresponds to the bounds of a virtual road user. The bounds are predicted as the worst case scenarios, corresponding to a constant velocity equal to zero or to the maximum allowed velocity v lim :

s(t 0 + ∆t) = s(t 0 ) s(t 0 + ∆t) = s(t 0 ) + v lim × ∆t (3)

C. LGM update

Once bounds of road users and hidden cells have been computed, the predicted LGM (t|t 0 ) is updated with this new information. For this purpose, we define the reachability function as follows.

Definition 4.1 Reachability function

The reachability function R defines the cells c of the predicted LGM (t|t 0 ) that are reachable. For each predicted entity (real or virtual road user) that at time t is contained within an interval s(t) = [s(t); s(t)], the set of reachable cells R(s(t)) in the predicted LGM (t|t 0 ) is defined as all the cells c that intersect s(t).

R(s(t))

= {c ∈ LGM (t|t 0 )|c ∩ s(t) = ∅} (4)
All the reachable cells are updated as such. Fig. 10 shows the prediction stages with the LGM update and the reachability function applied to two road users for a time horizon ∆t. Fig. 10a shows an LGM (t 0 ) that has cells with free, occupied and unknown states at time t 0 . Two vehicles V 1 and V virtual are considered (the ego vehicle is not represented). In Fig. 10b, the predicted bounds of the road users are represented with dotted lines. The cells that

encompass the prediction bounds of each vehicle are retrieved. In Fig. 10c, the LGM (t|t 0 ) is updated with reachable cells (orange). The prediction of hidden cells is similar to the prediction of objects, with the assumption that the length of a virtual road user corresponds to the size of the aggregated hidden cells.

D. Integrity: FNR evaluation

The integrity of the prediction is now addressed. The purpose is to show how the integrity of information can be controlled regardless of the performance of the prediction model. Let consider a road user prediction on a single lane. Table III shows the types misleading information that can be produced by the predicted LGM, analogously to the concept of integrity in the static LGM. Here, it is hazardous to define as non-reachable an area that in reality is occupied, since this may induce wrong decision-making.

To illustrate the management of integrity management we built a family of custom naive object predictors with constant unknown acceleration, providing the following curvilinear abscissa bounds:

s(t 0 + ∆t) = s(t 0 ) + v(t 0 ) × ∆t + 1 2 a(t 0 ) × ∆t 2 s(t 0 + ∆t) = s(t 0 ) + v(t 0 ) × ∆t + 1 2 a(t 0 ) × ∆t 2 (5)
where s is the curvilinear abscissa, v the velocity and a the acceleration under the hypothesis that the acceleration remains constant in the time interval [t 0 , t 0 + ∆t] and that the velocity is positive and bounded by v lim , the maximum allowed velocity. Note that this predictor could be replaced by any other predictor from the literature.

1) Simulated results:

As an initial step, to make it easier to understand the experimental results, we did some simulations. Given the LGM of a straight lane, we consider a single road user traveling in accordance with a Constant Velocity (CV) model, i.e. Eq. 5 is used with a = 0 m.s -2 . Then, to highlight the integrity management mechanism, we deliberately choose, for the road user prediction, an inaccurate Constant Deceleration model (CD), with a(t) ∈ [-1.5; -3.5 ]m.s -2 . At a given time t 0 , the predicted LGM (t|t 0 ) is generated over 4.0 s. This simulation allows us to compare directly the true evolution of a road user (which is known) with a given evolution model. The purpose is to show how, even with an approximate evolution model, the sampling step parameter can ensure the integrity of the prediction information.

Fig. 11 shows simulated results for the vehicle driving on a straight lane at constant speed (blue line) with the CD model. The figure shows to what extent predicted bounds encompass, or fail to encompass, the whole body of the vehicle at each prediction time. The vehicle's body is modeled by its position (the center) with an upper and lower bound (rear and front). The predicted bounds are represented in a continuous space dimension, which is equivalent to setting a sampling step close to zero. From Fig. 11a it will be remarked that the CD model does not encompass the true position as the time horizon increases. The goal is now to show how misleading information is managed.

Fig. 11d shows the smallest sampling step that enables the predicted reachable bounds to encompass the vehicle position for at least 2.0 s of prediction. Since the CD model does not bound the road user position, a sampling step of 5.0 m is chosen in order to overestimate the reachable area. From Fig. 11b it can be seen that a sampling step of 1.0 m fails to encompass the true position over the interval. As the sampling step increases (see Fig. 11c), the encompassing of the road user position improves. In stark contrast to Fig. 11a, and according to the target integrity risk, integrity is maintained for 2.0 s of prediction by using a larger sampling step. In a similar way, the sampling step may be increased in order to maintain integrity over a larger ∆t horizon.

If the prediction model is reliable for a short prediction time and then drifts, a sampling step can be set in accordance with the relevant time horizon. As a consequence, even with an extreme case such as the CD model, a very large sampling step means that the whole lane can be defined as reachable.

Here, although the information has a high degree of integrity, the decision-making process will clearly be impacted towards considerable caution, since there are no longer any free cells.

As mentioned before, the goal here is to ensure that any misleading information is non-hazardous. For this purpose, the integrity of the system is defined as its ability to maintain statistically the FNR below a given threshold that is termed the Target Integrity Risk (TIR).

Fig. 12a shows the FNR for the same situation, but obtained using a range of different sampling steps. Because the CD model is an inaccurate model here, the FNR grows rapidly as the prediction time horizon increases. It can also be observed that for any given time horizon, increasing the sampling step reduces the FNR. The kinks in the curves are a side effect of the sampling step. This is because the cells defined as reachable depend on t 0 . At this time, if the road user is close to the start or to the end of a cell, there will be an impact on the update function.

The true statistical FNR is obtained by averaging more than one thousand simulations varying the initial time t 0 . It can be seen from Fig. 12b that whatever the given time horizon, the FNR decreases by as the the sampling step increases. Given a fixed TIR, this figure highlights the integrity property of the sampling step, which is similar to the integrity of a static LGM. The further away the predicted time, the higher the sampling step that is required in order to remain below the TIR.

2) Experimental results: Three sequences were recorded in a dataset for a situation where the ego vehicle was observing another experimental vehicle entering a roundabout on its left side. Fig. 13 shows the experimental results that are representative of Fig. 12a with a CD model. The actual evolution model of the human road user is unknown, and the curves suggest that the CD model is likely to be inaccurate.

In Fig. 13a, the evolution of the FNR with the CD model is shown for different sampling steps from 0.2 to 5 m. For any required prediction time horizon and TIR, it is possible to maintain the FNR below the TIR by increasing the sampling step. When the prediction time horizon increases, the bounds computed by the CD model are further away from the true road user position, thus requiring a much larger sampling step in order for the correct cell to continue to capture the road user. In contrast, where the evolution model is closer to the true evolution, e.g with a CV model, the bounds computed by the model will likely contain the true road user position (see Fig. 13b). The LGM can therefore be computed with a much smaller sampling step. We recall that prediction model performance is not being evaluated here. If the prediction model is accurate enough to capture the road user position within the prediction bounds, then the requirement in terms of the sampling step will not be stringent. Conversely, if the prediction model is far removed from reality, the sampling step will act as a parameter for managing the integrity of the whole system.

However, increasing the sampling step can reduce the nonreachable space in the LGM. This results in an overestimation of the reachable space that can impact the decision-making towards an excess of caution. The consequence is a loss of functional availability for the autonomous navigation of a vehicle, which may, for example, remain stuck for a long time at an intersection when traffic is heavy.

V. ENHANCED SPACE CHARACTERIZATION

This section shows how prediction may be improved by taking interactions into account. In order to strengthen decisionmaking, including under occlusion, the available and missing information in areas of interest needs to be quantified. With the aim of improving situation understanding in unknown areas, such as areas hidden by other road users, we developed a characterization process that applies reasoning on the basis of prior map information. Below we explain the concept of neutralization in relation to these unknown areas.

Ego vehicle

Vx : Road users V1 V2 Fig. 14: LGM with a characterization of the neutralized cells on a T-intersection: Free (green), Occupied (red), Unknown (gray), Neutralized (purple).

A. Enhanced characterization process: the neutralized frame

An important aspect of the interaction between road users is that they impose physical constraints on each other. Once a road user occupies a given space on the road, no other road user can cross this space without causing an accident.

Fig. 14 presents a situation where a neutralized area is caused by a dynamic object. The vehicle V 2 is protecting the ego vehicle from road users coming from the left, e.g. V 1 . It is physically engaged and thus induces a neutralized area as shown in purple. It must be noted that the neutralized area may reach outside of the field of view of the ego vehicle. As it is neutralized by a perceived road user, the ego vehicle has the possibility to take a decision under occlusion. The neutralized property is only relevant within the unknown category. In Fig. 14, the neutralized property can be ignored where a perceived road user is present as the occupied knowledge is more informative.

B. Prediction with neutralization

When a neutralized area has been characterized in the current LGM, its status can also be predicted.

Suppose that there is an initial neutralized situation at time t 0 . As a consequence, during the prediction, a key point is to estimate how long the situation will remain with a neutralized lane, since this allows the ego vehicle to navigate while remaining protected. The duration of this neutralization is called Neutralized Time Interval (NTI).

In order to determine if cells are neutralized at a time t = t 0 + ∆t, we need to look for occupied cells that may cause the lane to be neutralized. Only a cell that is fully occupied can generate a neutralized area. It is therefore crucial to characterize the cells that are truly occupied during the reachability analysis.

1) Occupancy: Reachable cells can be occupied, with the U state, or truly occupied, with a state O. A cell is considered occupied if it is completely overlapped by the footprint of a road user placed at the bounds of s(t) (see Fig. 15). As a consequence, a partially occupied cell is considered only as reachable, since it is not actually fully occupied. Road user lengths L are supplied by the object tracker.

The footprint occupancy is defined as follows.

Definition 4.2 Footprint occupancy

For a given road user in the interval s(t), its footprint occupancy s L (t) is defined by:

s L (t) = [s(t) -L; s(t) + L] if s(t) + L > s(t) -L ∅ otherwise (6)
The predicted LGM (t|t 0 ) is updated with this information. An occupation function is defined as follows.

O(s L (t)) = {c ∈ LGM (t|t 0 )|c ⊆ s L (t)} (7)
All the predicted occupied cells are updated with the label Occupied. Fig. 15 illustrates a predicted LGM update using the occupation function, given a road user V 1 and a time horizon ∆t. The footprint of V 1 is placed at each end of the predicted bounds. The cells that are within the overlapping bounds are retrieved. Note that if the predicted interval s(t) is twice as long as the road user length L, then there is no occupancy. It will be remarked that hidden cells cannot generate occupancy, but only reachability, since prediction is done only with respect to virtual vehicles. There is no guarantee that these virtual vehicles correspond to real physical road users.

2) Neutralization: We now focus on the prediction of an LGM with a lane characterized initially as neutralized.

In this problem, applying a constraint on a vehicle prediction is dependent on the prediction of another road user. Prediction of cells located in a neutralized area is constrained if and only if this area remains neutralized. An area can remain neutralized only if there has been a prediction of another road user that has given rise to this situation. At first glance it would appear complicated to make predictions of road users that are dependent on others, since the number of predictions can increase dramatically in some complex driving situations. That is why all the road users are first predicted independently w.r.t. each other without any constraint. The physical interaction induced by neutralization is then back-propagated onto the reachable cells of the road users and the hidden cells located in the neutralized areas. This prevents prediction of some cells from reaching non-reachable areas.

For a predicted LGM (t|t 0 ), there are two possible scenarios:

a) The neutralized area remains neutralized in the predicted

LGM at time t 0 + ∆t (see Fig. 16). As a consequence, a constraint is applied on reachable cells which were generated by road users or potential road users in the neutralized area. This backward constraint propagation is defined as follows. Definition 4.4 Backward constraint propagation Let the set {s 1 (t 0 ), s 2 (t 0 ), . . .} be the set of entities

C = O(t)\N (8)
are actually not reachable by the entities in N . The backward constraint propagation consists in canceling all the updates that have been applied to the cells in C by any entity s i . As a result, in Fig. 16, the backward constraint propagation is applied on the cells that are located outside the neutralized area. The difference can be seen where cells are set to free instead of unknown. b) The neutralized area is no longer neutralized in the predicted LGM at time t 0 +∆t. No backward propagation is applied. 3) Integrity: This section is concerned with managing the integrity of the information provided by the predicted LGM, taking into account the neutralized situation. In comparison with the integrity study presented above (see Table III), there is a new hazardous situation (see Table IV). Up to now, a false positive, that is predicting a free cell as occupied or reachable, was considered non-hazardous, given that any resulting inappropriate behaviour is likely to be excessively cautious rather than insufficiently so. With the introduction of the concept of neutralization, a cell that is wrongly predicted as occupied may lead to false neutralization constraints, which can imply hazardous decisions.

As a consequence, the F N R metric that we have used so far is no longer sufficient to guarantee system integrity. An additional metric is needed to address this new hazardous situation. For this purpose we compute the duration of the neutralization constraints, termed NTI (Neutralized Time Interval). This performance metric corresponds to the last predicted time t N T I = t 0 + N T I for which a neutralized area remains. Computing the NTI when a neutralized area occurs means finding the earliest time for which there is no longer any fully occupied cell in the crossing area. The implementation consists in predicting the LGM over a time horizon with a given sampling time δt and simply identifying the first time when the neutralization ends. It corresponds to a lower bound approximation of the actual NTI.

The goal is to compute a value that is as close as possible to, but does not exceed, the real NTI. Overestimating the NTI may lead to hazardous situations, e.g. the ego vehicle estimates that it has enough time to cross while in reality it does not. Conversely, underestimating the NTI leads to Fig. 18: LGM representation at a roundabout in a real-time experiment in Compiègne. The ego vehicle (blue) enters the roundabout with a vehicle on its left side (red). This vehicle located on the secondary order lane causes an occlusion. As it is already engaged, the primary order lane is neutralized (purple). Free cells are shown in green, and occupied cells in red. Other cells in gray are partially or completely hidden. overcautious decisions: the integrity of navigation information is guaranteed, but the availability of the navigation function is reduced.

The quality of the estimated NTI depends on the prediction of the road users. For example, in Fig. 17, a mistakenly neutralized area is assumed with a sampling step of 1 m. The upper bound of the road user V 2 has been underestimated. In Fig. 17a, at some time t in the future, the LGM predicts that a cell will be fully occupied when it is not. This generates a neutralized area that may lead the ego vehicle to decide to cross. This situation leads to an overestimation of the NTI.

We have already demonstrated that enlarging the sampling step provides a means of handling system integrity in terms of F N R. Fortunately, the same process can also be used for controlling integrity in terms of N T I. As the sampling step increases, for a given prediction time, the state of an occupied cell changes to unknown, meaning that potentially a wrongly neutralized area may disappear, once there are no remaining occupied cells (see Fig. 17b).

C. Real experimental results

1) Implementation: The LGM generation and its characterization process were implemented on a Renault ZOE experimental vehicle belonging to the Heudiasyc Laboratory using ROS middleware. The computer used had an Intel i7 (7th) processor and 16 Go of RAM. For the computation of the IG, a distance of interest of 100 m was chosen, and for the LGM an arbitrary cell discretization step of 1 m was set in order to achieve real-time performance. The LGM was generated at a frequency of 10 Hz and the IG at 0.25 Hz.

2) Case study: In Fig. 18, a neutralized area is shown in a roundabout entrance situation. The ego vehicle intends to enter the roundabout by merging with the outer lane. Since the vehicle on the left belongs to the lane to the left of the ego vehicle and is crossing the merging lane, the area behind that vehicle is therefore characterized as neutralized. This is the situation addressed in our computation of LGM (t 0 ).

3) Results: To compute the bounds for a given prediction ∆t horizon, Eq. 5 is used with different intervals for the acceleration. The following three models, labeled according to their upper bound on the acceleration, are considered in order to evaluate their impact on the results: For each model, the velocity is also constrained within the interval [0; v lim ], i.e. the vehicle cannot travel backwards or faster than the speed limit (where v lim = 50.0 km.h -1 in the roundabout). They all have the same lower acceleration bound. The model used for hidden cells is based on Eq. 3 where v lim = 50.0 km.h -1 . We therefore have a constant velocity model.

In order to store the predicted LGM, each cell of the LGM has a vector of its predicted states. When a neutralized area is encountered, as described above, the prediction process is launched. Given a maximum prediction time horizon ∆t max = 2 s and a time sampling step of 0.1 s, each predicted LGM (t|t 0 ) is computed based on the initial LGM (t 0 ) and the neutralized constraint is applied depending on the situation. The neutralized constraint is back-propagated if and only if the neutralized area remains neutralized. This gives the predicted NTI. Once the constraint is no longer applicable, the predicted NTI is used for the reachability analysis of the neutralized cells. Cells outside the neutralized area start to be reached after the NTI has elapsed.

In order to show how integrity is handled through the prediction process, a number of different sampling steps were used for the predicted LGM.

The results that are presented in this section serve a twofold purpose. First, they demonstrate the advantage of the neutralized constraint. It is possible to make a prediction that remains consistent and that is able to improve the reachability analysis, and thus the decision-making process, throughout the NTI. Second, they show that an appropriate tuning of the sampling step parameter can prevent this prediction representation from generating misleading information.

The impact of the sampling step parameter on the predicted neutralized time interval is shown in Fig. 19. The different curves represent the evolution of the NTI using different prediction models for the road users: CD (blue), CV (green), CA (red). With all three models it can be seen that that higher the sampling step, the lower the predicted NTI.

The NTI obtained with the LGM (t) observed in replay is shown as a horizontal gray line. A misleading situation occurs where the estimated NTI is larger than the true NTI, as in the case of the CD model. By increasing the sampling step, the NTI is no longer misleading, even with an approximate model. The CV model is able to maximize the NTI, while the CA model is much more conservative but also more robust. With a more pessimistic model, it becomes harder to predict a long NTI, and the resulting situation understanding is therefore more conservative. Results show how the sampling step can maintain integrity by ensuring a lower estimation of the predicted neutralized time interval. The CD prediction model demonstrates the loss of integrity when a model does not bound the real behavior of road users. In order to fulfill the same requirement, the sampling step needs to be increased up to two meters at least. It will be remarked that the maximum NTI that can be obtained is bounded by the maximum predicted time horizon, ∆t = 2 s in this case.

VI. CONCLUSION

The Lane Grid Map is a lane-level representation that allows the driving situation of an autonomous vehicle to be modeled at an intermediate level by taking into account topological information from an HD map. Since this representation is used by a decision-making system, the integrity of the information that it contains needs to be controlled over a time horizon in order to avoid providing misleading information. We have shown how an LGM can make a prediction model reliable, even if it is inevitably an imperfect reflection of reality. This is achieved by setting an appropriate sampling step for the LGM, such as to maintain the integrity below a given Target Integrity Risk. The general principle is that the False Negative Rate can be reduced as needed by increasing the length of the sampling step. This strategy can lead to a loss of availability of the driving system as the vehicle becomes overcautious. For this reason, we have proposed an improvement that takes interactions into account interactions via the concept of neutralization, which can improve decision-making in some situations without compromising safety. Interactions are taken into account with the NTI metric, and we have shown that a similar sampling step tuning procedure is also able to guarantee the of the prediction in these situations.

Once an object predictor has been set, along with the integrity requirement of the decision-making module in terms of prediction time horizon and TIR, a data collection step is required in order to tune the integrity performance and select the optimal sampling step satisfying the integrity requirement while maximizing the availability. Because this strategy is data driven, finding a robust optimal sampling step requires a large quantity of recorded driving recording data for all the target operational domain designs.

In this paper, we have considered a simplified LGM with a space discretization in the along-track direction and with a single sampling step. In future works, we will consider both the along-and cross-track directions, as well as sampling steps that are adaptive w.r.t. to the navigation task, in order to further increase the availability of the system.

Fig. 3 :

 3 Fig. 3: Lanes of interest in a real experiment led in Compiègne. The ego vehicle is represented in blue and its intended path represented by the blue line. The red line represents lanes in direct interaction with the ego vehicle path and the orange lines are lanes that interact indirectly.

Fig. 4 :

 4 Fig. 4: Left: an LGM for a merging intersection. The ego vehicle (blue) intends to turn right. Right: A Cartesian OG.

Fig. 5 :

 5 Fig. 5: Characterization process with polygons supplied by a perception module: free in blue, occupied in red and unknown in gray. The skeleton of the LGM is shown above, and the final characterization below.

Fig. 6 :

 6 Fig. 6: Experimental vehicles (white car and gray truck behind the blue car) in Compiègne (France).

Fig. 7 :

 7 Fig. 7: Ground truth (above) and observed (below) LGMs.

Fig. 9 :

 9 Fig.9: FNR of the LGM with respect to the spatial sampling with different levels of noise on the positions of the detected vehicles.

Fig. 10 :

 10 Fig. 10: Processes for building a predicted LGM (t 0 + ∆t|t) with a virtual and a real road user. (a) LGM (t 0 ) with free (green), hidden (gray) and occupied (red) cells. The rear boundary of vehicle V 1 is represented by a dotted line. (b) Upper and lower bounds of the predicted road users. (c) The predicted LGM is updated in orange with possibly occupied states.

Fig. 11 :

 11 Fig. 11: The road user position (blue) is modeled with an upper and lower bound (rear and front). The reachable distance is filled by the purple area. The lower bound of the CD model corresponds to the black curve and the upper bound to the red curve. (a) reachable area with a sampling step that tends to 0.0 m (b) reachable area with a sampling step of 1.0 m, (c) 2.0 m, (d) 5.0 m.

Fig. 12 :

 12 Fig. 12: Evolution of the FNR w.r.t. the time horizon for different sampling step using simulated data (CD model). (a) Single simulation. (b) Average over more than one thousand simulations. The further away from the origin, the more the curve corresponds to a distant predicted time horizon.

Fig. 13 :

 13 Fig. 13: (a) Evolution of the FNR of prediction with experimental results obtained with the CD model. Each curve displays the evolution for a given sampling step. (b) Evolution of the FNR of prediction for the CV and CD model with a sampling step of 0.2 m.

Fig. 15 :

 15 Fig. 15: Illustration of prediction stages with occupancy prediction at time horizon ∆t. (a) Model-based object prediction with upper and lower bounds and with an overlap. The footprint of the vehicle is shown at the two ends of the reachable set. (b) Final predicted LGM (t 0 + ∆t|t 0 ) with an additional occupied state in red.

V2Fig. 16 :

 16 Fig.16: Backward propagation of the neutralized constraint on a predicted LGM at time t 0 + ∆t. The purple area remains neutralized since V 2 is guaranteed to occupy at least one cell (in red) of the crossing area during the entire prediction horizon. The hatched area is therefore removed, since these cells are no longer reachable.

Fig. 17 :

 17 Fig. 17: Two LGM (t|t 0 ) computed with different sampling steps. As the sampling step increases, the misleading information due to a prediction error disappears. The situation is therefore no longer considered neutralized.

1 )

 1 Constant acceleration (CA): a(t) ∈ [-3.5; 4.0] m.s -2 2) Constant velocity (CV): a(t) ∈ [-3.5; 0.0] m.s -2 3) Constant deceleration (CD): a(t) ∈ [-3.5; -1.5] m.s -2

Fig. 19 :

 19 Fig. 19: Evolution of the predicted NTI obtained as a function of the sampling step. The three models CA, CV and CD are represented respectively by the red, green and blue curves. The NTI computed with the LGM (t) is shown as the dotted line. Values above this line are considered misleading.

Corentin

  Sanchez obtained his Master's degree in mechanical engineering from the Université de technologie de Compiègne (UTC), France, in 2017. He was awarded his Ph.D. degree by UTC in 2022. Since 2018 he has been a member of SIVALab, a laboratory run jointly by Renault, UTC and CNRS. His research interests cover self-driving vehicles, situation understanding, integrity and environment perception applied to autonomous navigation. Philippe Xu received his M.S. degrees in computer science from the Ecole Normale Supérieure de Cachan, France, in 2011. He received his Ph.D. degree from the Université de Technologie de Compiègne (UTC), France, in 2014. Since 2015 he has been an associate professor in the Computer Science department of UTC and is carrying out his research in Heudiasyc UMR 7253 (UTC/CNRS). Since 2017 he has also been a member of SIVALab. His research interests cover information fusion, computer vision and machine learning applied to localization and perception for autonomous vehicles. Philippe Bonnifait (Member, IEEE) received the Ph.D. degree in automatic control and computer science from the Ecole Centrale de Nantes, France, in 1997. He is currently a Professor with the Computer Science and Engineering Department, Université de Technologie de Compiègne, France. Since 1998 he has been part of Heudiasyc UMR 7253, a joint UTC-CNRS research laboratory. His research interests include intelligent vehicles navigation, in particular localization, mapping and perception of the environment using multi-sensor data fusion. Alexandre Armand received his Ph.D. degree in computer science and robotics from ENSTA Paristech, France, in 2016. His research was done within the ADAS department at Renault, focusing on risk assessment. He then joined Renault's research division to work on autonomous driving technologies, with a particular interest in world modeling and situation understanding.

TABLE I :

 I Construction table for the indicators: True/False

			Observation
	Ground Truth	Occupied	Free	Unknown
	Occupied	T P	F N	U P
	Free	F P	T N	U N

(T/F), Positive/Negative (P/N), and Unknown (U). (red: misleading and hazardous, green: non-misleading, gray: undetermined, yellow: misleading but non-hazardous)

TABLE II :

 II Aggregation rule for two cells. Cell res corresponds to the result of the fusion of the cells Cell i and Cell i+1 .

TABLE III :

 III Correspondence between the predicted states ofLGM (t|t 0 ) with the true states of LGM (t).

	True	Predicted LGM (t|t 0 )
	LGM (t)	Reachable	Non-reachable
	Occupied	Non-Misleading (1)	Hazardous (2)
	Free	Non-Hazardous (3)	Non-Misleading

TABLE IV :

 IV Correspondence between the predicted states of LGM (t|t 0 ) and the true states of LGM (t). at time t 0 . At a given time t = t 0 + ∆t, let O(t) = R({s 1 (t), s 2 (t), . . .}) be the reachable cells of all these entities. Assuming that N is still neutralized in the predicted LGM (t|t 0 ), all the cells in O(t) that are outside the neutralized area N defined as

	True		Predicted LGM (t|t 0 )
	LGM (t)		Occupied	Reach.	Non-reachable
	Occupied		Non-Misleading (1)		Hazardous (2)
	Free	Hazardous (neutr.)	Non-Hazardous (3)	Non-Misleading (4)
	(real or virtual road users) that are in the neutralized
	area N

ACKNOWLEDGMENTS

This work has been carried out within SIVALab, a shared laboratory between Renault and Heudiasyc. Equipment from ROBOTEX (ANR-10-EQPX-44-01) were used for the experiments. The authors would like to thank Antoine Lima, Stefano Masi and Stéphane Bonnet for their support in the experiments.


Joelle Al Hage

Nicolò Salvatico

Philippe Bonnifait

High Integrity Localization of Intelligent Vehicles with Student's t Filtering and Fault Exclusion

High-integrity localization is a key element for safety-critical applications like autonomous driving. The navigation filter plays a crucial role in merging sensor data to estimate an accurate pose and calculate a confidence interval based on task requirements. This paper presents an end-to-end Student's t information filter for accurate data fusion and non-pessimistic confidence domain computation. The filter incorporates a Fault Detection and Exclusion stage based on the Kullback-Leibler Divergence. The degree of freedom of the t distribution shapes the heavy tail to make the estimation process more robust against non detected outliers. We show that the adjustment of the degree of freedom can be done in real time using measurement residuals which give an indirect vision of the environment complexity. The accuracy and integrity of the proposed approach are evaluated with real data acquired with an experimental vehicle using GPS and Galileo pseudoranges merged with camera measurements after a map matching step with a High-Definition map. A comparative study with other classical methods based on Kalman filtering is also reported.

I. INTRODUCTION

Localization with high integrity is a major challenge for safe navigation of intelligent vehicles. This requires the localization system to issue a safety alert when it should not be used. Vehicle localization on roads relies on the fusion of data collected from multiple sensors, including Inertial Measurement Unit (IMU), Global Navigation Satellite Systems (GNSS), cameras, and Lidars [START_REF] Li | Multi-gnss ppp/ins/vision/lidar tightly integrated system for precise navigation in urban environments[END_REF]. Furthermore, the incorporation of a High-Definition (HD) map is crucial to associate perception measurements with georeferenced features [START_REF] Quddus | Current mapmatching algorithms for transport applications: State-of-the art and future research directions[END_REF]- [START_REF] Ghallabi | Lidar-based high reflective landmarks for vehicle localization in an hd map[END_REF].

Merging data from multiple sensors is usually done using a Kalman Filter (KF). However, the assumption of Gaussian distributions is not always justified and can be problematic when bounding the errors under small probabilities. This principle is linked to the integrity concept.

The integrity of intelligent vehicles, that originates from the aviation domain, is today an active research area that aims to reduce the probability of accidents as much as possible [START_REF] Zhu | GNSS position integrity in urban environments: A review of literature[END_REF], [START_REF] Margaria | The local integrity approach for urban contexts: Definition and vehicular experimental assessment[END_REF]. It refers to the trust that can be placed in a navigation solution and is associated with a target integrity risk (T IR) that represents the maximum probability that the error in position exceeds a limit without warning the user [START_REF] Zhu | GNSS position integrity in urban environments: A review of literature[END_REF], [START_REF] Hage | Localization integrity for intelligent vehicles through fault detection and position error characterization[END_REF]. The error bound (or confidence interval) is known as the Protection Level (PL) and is computed so that the probability of the position error exceeding it is less than the T IR.

In this paper, the aim is to achieve a high integrity localization while maintaining a lane-level accuracy. The PL values must be determined in real time without an underestimation due to biases, non Gaussian distributions or errors resulting from the linearization of models. The Student's t filter (StF) which is a generalization of the KF is well suited to these problems since it handles heavy tailed distributions [START_REF] Roth | A student's t filter for heavy tailed process and measurement noise[END_REF]. Multiple adaptation of this filter can be found in the literature [START_REF] Huang | A novel robust student's t-based kalman filter[END_REF]- [START_REF] Tronarp | Sigma-point filtering for nonlinear systems with non-additive heavy-tailed noise[END_REF].

The heavy tail property is important to bound the errors in challenging environments. In this paper, an end-to-end approach based on Bayesian Student's t filtering is proposed for data fusion and simultaneous PL computation. Pseudoranges from different GNSS constellations and measurements from a smart camera are modeled as Student's t distribution and are merged with odometery data through a Tightly Coupled (TC) architecture [START_REF] Liu | Robust state and protection-level estimation within tightly coupled GNSS/INS navigation system[END_REF]. The integration of low-level GNSS measurements in data fusion allows for improved modeling, resulting in superior integrity performance compared to a loosely coupled scheme that relies on the position solution provided by a GNSS receiver. Although the StF is more robust than KF, large errors still affect the localization accuracy and integrity. Therefore, a Fault Detection and Exclusion (FDE) based on the Kullback-Leibler Divergence (KLD) is proposed to remove the erroneous measurements from the data fusion. The Student's t Information Filter (StIF), proposed in our previous work, is used to simplify the fault exclusion [START_REF] Hage | Student's t information filter with adaptive degree of freedom for multi-sensor fusion[END_REF]. Mathematically equivalent to the StF, the key aspect of the StIF lies in its update step, which is modeled as a summation of the information contributions. Unlike [START_REF] Hage | Student's t information filter with adaptive degree of freedom for multi-sensor fusion[END_REF], in this paper, raw measurements are modeled as Student's t distributions to improve the integrity performance.

The StF is based on t-distributions characterized by a Degree of Freedom (DoF) which influences the tails to be lighter or heavier. When the DoF approaches infinity, the t-distribution becomes similar to a Gaussian. The DoF has a large influence on the filter and on the confidence interval values. An incorrect setting can lead to suboptimal behavior. In certain studies such as [START_REF] Roth | A student's t filter for heavy tailed process and measurement noise[END_REF], a choice is made to set the DoF equal to three to preserve heavy tails. However, this particular choice is not suitable for our application, considering the dynamic nature of the environment. For instance, opting for this value in an open sky environment can lead to a degradation of the filter results. In [START_REF] Li | Robust selection of the degrees of freedom in the student'st distribution through multiple model adaptive estimation[END_REF], the authors deal with this problem through a robust Multiple Model Adaptive Estimation (MMAE) where a bank of three StF with different DoF values is used. The MMAE selects the best filter or combine the outputs of the different StF. This method limits the number of possible choices on the DoF. In this paper, we propose to adapt the DoF accordingly to the residual computed for the fault detection. This residual includes information about the quality of pseudoranges and camera measurements.

This paper is organized as follows: section II presents the proposed multi-sensor data fusion through a tightly coupled architecture using the student's t information filter. Section III presents the fault detection and exclusion step based on the KLD and the integrity study. Experimental results and discussions based on real data are given in section IV.

II. MULTI-SENSOR DATA FUSION USING GNSS PSEUDORANGES AND CAMERA MEASUREMENTS

A. System modeling

Let's consider a vehicle equipped with wheel-speed sensors, a yaw-rate gyro, a GNSS receiver and a smart camera for lane marking detection. The localization problem is defined as estimating the vehicle pose with respect to a local ENU (East-North-Up) frame (R O in figure 1). The vehicle pose is defined at the body frame located at the middle of the rear axis (R B ). For a tightly coupled data fusion scheme, the state vector at epoch k is defined as:

X k = [x, y, z, θ, cdt, c ḋt] T k . (1)
The receiver clock offset cdt (multiplied by the speed of light c) and clock drift c ḋt with respect to GPS time are part of the state vector and are estimated within the filter, along with the pose (θ is the heading). When dealing with two constellations, e.g. GPS and Galileo, to avoid adding variables to the state vector that represent receiver clock bias and drift with respect to Galileo system time, the estimate of the Galileo to GPS Time Offset (GGTO) can be used [START_REF] Bo | Satellite selection algorithm for combined gpsgalileo navigation receiver[END_REF]. The evolution model involves odometric data as input:

X k+1 = f (X k , u k ) + v k , (2)
where u k = [∆ k , Ω k ] is composed of the elementary displacement and rotation obtained from the wheel-speed sensors and the gyro respectively. The process noise v k is modeled as a Student's t distribution with zero mean, DoF γ and scale matrix

Q: v k ∼ St(0, Q k , γ k ). (3)
If a Gaussian distribution is sufficient for modeling v k , a high value of γ that tends to infinity can be chosen. Consider m pseudoranges acquired at epoch k, the corrected pseudorange ρ j can be written as:

ρ j = R j + cdt + ϵ, (4)
where R j is the Euclidean distance between the receiver and satellite j and ϵ is the receiver noise.

As pseudoranges link the satellites to the receiver antenna located at the roof of the vehicle (R G ), the transformation between the antenna and the body frame is used: The Euclidean distance is written as:

  x y z   =   x y z   GN SS -   cos θ -sin θ 0 sin θ cos θ 0 0 0 1     t x t y t z   . (5) x O y O R O 𝑥 𝑦 x B y B x G y G x M y M L A B 𝑃 𝑥 𝜃 R B R G R M
R j = (x + t x cos θ -t y sin θ -x j ) 2 + (y + t x sin θ + t y cos θ -y j ) 2 + (z + t z -z j ) 2 (6)
where (x j , y j , z j ) are the coordinates of satellites j in the ENU frame, (x, y, z) GN SS corresponds to the antenna position and (t x , t y , t z ) is the translation of the antenna with respect to the body frame.

Given equation 4, the model of ρ is expressed as:

Z k = h k (X k ) + ω k , (7)
where ω k is the noise associated to the pseudorange measurements modeled as a Student's t distribution with zero mean, DoF δ and scale matrix R:

ω k ∼ St(0, R k , δ k ). (8)
We take advantage of this modeling to improve the integrity of the state estimation. The details will be shown in the next section.

Beside GNSS measurements, another observation model is used for lane measurements from a smart camera. The camera used is able to detect up to four lane markings at a time (two at each side) and return lateral distances denoted C 0 (figure 1). C 0 refers to the signed distance between R M and the intersecting point L at the map-matched segment [AB] of the HD map. The camera observation is expressed as [START_REF] Tao | Road-centered map-aided localization for driverless cars using single-frequency GNSS receivers[END_REF]:

C 0 = (P x sin θ + y -y A )x AB -(P x cos θ + x -x A )y AB x AB cos θ + y AB sin θ .
(9) P x is the distance between R B and R M . For more details, the reader can refer to [START_REF] Hage | Localization integrity for intelligent vehicles through fault detection and position error characterization[END_REF]. As in the case of GNSS, the noise ω k associated to the camera measurements is modeled as a Student's t distribution to improve the integrity in presence of poor perception data.

Given these models, the state filtering is presented in the following.

B. State filtering

Let X be a random variable that follows a multivariate Student's t distribution: X ∼ St(µ, P, ν) where µ is the mean vector, P is the scale matrix and ν is the degree of freedom (DoF) [START_REF] Kotz | Multivariate t-distributions and their applications[END_REF]. The covariance is defined when ν > 2 and is linked to the scale matrix by Σ = ν ν-2 P . The development of Student's t filters is more challenging than Kalman filters, as careful approximations must be taken into account to avoid undesired behaviors [START_REF] Roth | A student's t filter for heavy tailed process and measurement noise[END_REF]. In this paper, the informational form of the Student's t filter (StIF) developed in [START_REF] Hage | Student's t information filter with adaptive degree of freedom for multi-sensor fusion[END_REF] is used. The StIF is equivalent to the StF but its advantage appears in the update step thanks to the summation of the information contributions that make it suitable for multi-sensor data fusion and for fault detection and exclusion.

The distribution of noises are described as Student's t distributions as given in equations 3 and 8. The initial state is also described as a t distribution: St(X 0 , P 0 , ν 0 ) where X 0 , P 0 and ν 0 are the initial guesses.

Compared to the StF, the StIF features an information matrix (up to a constant) and an information vector computed from the scale matrix and the state vector as [START_REF] Hage | Student's t information filter with adaptive degree of freedom for multi-sensor fusion[END_REF]:

Y k = P -1 k , (10)
y k = Y k X k . (11)
These equations are obtained in a similar manner to the informational form of the KF, known as the Information Filter (IF) [START_REF] Durrant-Whyte | Multisensor data fusion[END_REF]. The StIF includes two steps: time update and measurement update.

At time k, suppose that p(X k |Z 1:k ) follows a t distribution St(X k|k , P k|k , ν k ) and p(v k ) is given in equation 3. To derive the prediction step of the StIF, these two distributions must share a common DoF denoted ν ′ k [START_REF] Roth | A student's t filter for heavy tailed process and measurement noise[END_REF], [START_REF] Roth | Robust bayesian filtering and smoothing using student's t distribution[END_REF]. In order to preserve the heaviest tail between the two distributions, ν ′ k may be chosen as [START_REF] Roth | Robust bayesian filtering and smoothing using student's t distribution[END_REF]:

ν ′ k = min(ν k , γ k ). (12)
In our case, the heavy tail property is important to ensure the integrity of the state estimation. Given this new DoF, the scale matrices P k|k and Q k must be adjusted in P ′ k|k and Q ′ k . This can be obtained by moment matching or by minimizing the Kullback-Leibler divergence [START_REF] Roth | Robust bayesian filtering and smoothing using student's t distribution[END_REF]. In this work, the matrix adjustment is done by minimizing a modified upper bound of the KLD as proposed in [START_REF] Huang | A novel kullback-leibler divergence minimization-based adaptive student's t-filter[END_REF].

Using a first-order Taylor approximation and given that linear transformations preserve the DoF of a Student's t distribution [START_REF] Kotz | Multivariate t-distributions and their applications[END_REF], the predicted distribution can be approximated as:

p(X k+1 |Z 1:k ) ∼ St(X k+1|k , P k+1|k , ν ′ k ), (13)
where

X k+1|k = f (X k|k , u k ), (14)
P k+1|k = F k P ′ k|k F T k + Q ′ k . ( 15
)
which can be written in the informational form:

Y k+1|k = P -1 k+1|k , (16)
y k+1|k = Y k+1|k X k+1|k , (17)
where F k is the Jacobian matrix. Likewise, to derive the measurement update, a choice on the DoF must be made. Keeping the same policy as in the prediction step, ν ′′ k may be chosen as:

ν ′′ k = min(ν ′ k-1 , δ k ). (18)
Then:

p(X k , w k |Z 1:k-1 ) = St X k|k-1 0 , P ′ k|k-1 0 0 R ′ k , ν ′′ k ( 19
)
where P ′ k|k-1 and R ′ k are the adjusted matrices of P k|k-1 and R k given ν ′′ k . The posterior distribution p(X k |Z 1:k ) is then expressed as a Student's t distribution St(X k|k , P k|k , ν k ) and the updated parameters in the informational form are given by [START_REF] Roth | A student's t filter for heavy tailed process and measurement noise[END_REF], [START_REF] Li | Robust selection of the degrees of freedom in the student'st distribution through multiple model adaptive estimation[END_REF], [START_REF] Hage | Student's t information filter with adaptive degree of freedom for multi-sensor fusion[END_REF]:

ν k = ν ′′ k + d Z , (20)
Y k|k = c -1 k (Y k|k-1 + N i=1 I i,k ), (21)
y k|k = c -1 k (y k|k-1 + N i=1 i i,k ), (22)
c k = ν ′′ k + ∆ 2 k ν ′′ k + d Z , (23)
where d Z is the dimension of the observation vector, N is the number of observations assumed to be uncorrelated with each other,

∆ 2 k = (Z k -h(X k|k-1 )) T S -1 k (Z k -h(X k|k-1 )) is the Mahalanobis distance and S k = H k P k|k-1 H T k + R k is the innovation scale matrix. I i,k = H T i,k R -1 i,k H i,k and i i,k = H T i,k R -1 i,k Z i,k
are the information contributions of the observation Z i,k . Since the observation model is non linear, i i,k is expressed as:

i i,k = H T i,k R -1 i,k [Z i,k -h i (X k|k-1 ) + H i,k X k|k-1 ],
where

H i,k = ∂hi ∂X | X=X k|k-1
is the Jacobian matrix corresponding to the measurement model Z i (pseudorange or camera measurements).

The updated equations are similar to the IF, but scaled by a term c k that depends on the Mahalanobis distance between the measurement and its prediction. Therefore, it enables the filter to be aware of the observation quality and to scale the overall measurement update. Hence, if the measurement differs significantly from its prediction, an increase in the covariance matrix appears.

From equation 20 and after the first update step, the DoF increases to ν k . Without the limitation applied in the prediction step (equation 12), the DoF would tend to infinity and the 𝑋 𝑘/𝑘 (1) , 𝑌 𝑘/𝑘

𝜈 𝑘

Fig. 2: General architecture of the proposed method. v and w are the process and measurements noises respectively, ν the DoF, X the state vector and P the scale matrix. C 0 the measurements from the camera. St for Student's t.

StIF would converge to an IF. Regarding the initial values of γ, δ and ν ′ 0 , they are assumed to be equal and this choice is maintained over time. Therefore, ν ′′ k is equal to ν ′ k-1 (equation 18) and no approximation is needed when going from time update to measurement update. However, a bad value of the DoF affects the accuracy and integrity of the estimate. In order to avoid setting a fixed DoF, especially when the environment changes, a strategy to define it is presented in the next section by taking advantage of the FDE step.

III. HIGH INTEGRITY FOR MULTI-SENSOR DATA FUSION

A. Fault detection and exclusion

Although the StIF is designed to be more robust than the IF in the presence of faults, the latter still influence the quality of the filter estimates. For this purpose a fault detection and exclusion able to exclude faulty measurements is proposed.

The generation of a residual is the first step of an FDE where the KLD between the updated and the predicted distributions is used. As discussed before, the predicted distribution p(X k |Z 1:k-1 ) follows a t distribution with mean X k|k-1 , DoF ν ′ k-1 and scale matrix P k|k-1 (equation 15). The updated distribution p(X k |Z 1:k ) follows a t distribution with mean X k|k , DoF ν k and scale matrix P k|k (inverse of equation 21).

To simplify the equation of the KLD, we propose to approximate the original distributions by Gaussian distributions. Let q 1 and q 2 be the approximated Gaussian distributions of the predicted and updated distributions, respectively. X k|k-1 and

ν ′ k-1 ν ′
k-1 -2 P k|k-1 are the mean and covariance of q 1 . Likewise, X k|k and ν k ν k -2 P k|k are the mean and covariance of q 2 . This approximation is sufficient for residual computation.

Given these covariance matrices, the corresponding information matrices are:

Y Σ,k|k-1 = ( ν ′ k-1 ν ′ k-1 -2 P k|k-1 ) -1 , (24)
Y Σ,k|k = ( ν k ν k -2 P k|k ) -1 . (25)
The residual r k is defined as the KLD of the predicted distribution q 1 from the updated distribution q 2 (KLD(q 2 ||q 1 )) and is given by [START_REF] Do | Fast approximation of kullback-leibler distance for dependence trees and hidden markov models[END_REF]:

r k = 1 2 [(X k|k -X k|k-1 ) T Y Σ,k|k-1 (X k|k -X k|k-1 ) + trace(Y Σ,k|k-1 Y -1 Σ,k|k ) + log |Y Σ,k|k | |Y Σ,k|k-1 | -d], (26)
where d is the dimension of the state vector. For this computation, only [x, y, z, θ] are used, then d = 4.

The residual given in equation 26 acts in the state space and takes into account two types of information: the Burg matrix divergence and the Mahalanobis distance [START_REF] Hage | Multi-sensor fusion approach with fault detection and exclusion based on the kullback-leibler divergence: Application on collaborative multi-robot system[END_REF]. Using this residual, we are not interested by detecting a fault that has no direct effect on the pose estimation. Likewise, two types of tests are taken into account: test on the mean and test on the covariance matrices.

It should be noted that the KLD is computed with respect to the updated distribution because the StIF increases the covariance in the presence of fault. Therefore, the information lost when the prediction is used to approximate the update is more important than the opposite case.

Once the residual r k for the current epoch is generated with a filter (called main filter) that uses all available observations, it must be compared to a threshold T h in order to complete the detection phase. The residual follows a distribution related to Chi-squared and F-distributions as in [START_REF] Hage | Multi-sensor fusion approach with fault detection and exclusion based on the kullback-leibler divergence: Application on collaborative multi-robot system[END_REF]. The threshold is then set according to a false alarm probability. If r k > T h, faults are detected and must be isolated before delivering the state estimate. For this purpose, a bank of filters is generated wherein every StIF does the measurement update using only one observation Z j (camera or GNSS pseudorange):

Y j,k|k = c -1 j,k (Y k|k-1 + I j,k ), (27)
y j,k|k = c -1 j,k (Y k|k-1 + i j,k ), (28)
X j,k|k = Y -1 j,k|k y j,k|k . ( 29
)
The residual r j,k associated to each observation is then computed in a comparable manner to equation 26.

Once these computations are performed, the maximum residual r j is found and is compared to the corresponding threshold T h j (the false alarm is not necessarily the same for each type of sensor). If the residual exceeds the threshold, the considered observation is excluded from the main filter. After the fault exclusion, the filter estimate is computed again and the FDE is repeated until no more faults are detected using the main filter (figure 2).

Concerning the StIF, measurement Z j is excluded from the fusion procedure by subtracting its information contribution (I j,k , i j,k ) from the main filter and by updating the c term.

After the FDE step, the fixed DoF given in equation 12 is replaced by an adaptive DoF that changes according to the residual values. Indeed, the residual provides an indication about the environment and the quality of the measurements. When this residual is large, measurements are likely to be noisy and tails should be heavier, so the DoF has to decrease. However, for small residual values, a lighter tail may be reasonable, so the DoF increases. Therefore, the adaptation of the DoF makes use of the residual computed by the fault detection phase, according to a negative exponential model with a minimum value:

ν ′ k = a. exp(b.r k ) r k < l ν ′ k = 3 r k > l (30)
where a, b and l are tuning parameters. For numerical stability and to be able to compute a covariance matrix, the minimum allowed DoF is chosen equal to 3. The general architecture of the method is given in figure 2.

B. Protection level computation

One of the objectives of using Student's t modeling is to bound in real-time the estimation errors without underestimating the PL bound. When dealing with localization integrity, a PL must be able to handle small target integrity risk (e.g. T IR = α ≤ 10 -3 ). The heavy tail property is fundamental for such a computation. So far, there is no consensus about the T IR value which should depend on the level of autonomy and operational design domain. To bound the errors, the matrix P k|k obtained in the measurement update is adjusted to P ′ k|k according to the minimum between ν ′ k (equation 30) and ν k (equation 20):

N 1 = min(ν k , ν ′ k ). (31)
Note that if the minimum corresponds to ν k , no adaptation is needed. The PL is then expressed as [START_REF] Madrid | Device and method for computing an error bound of a kalman filter based gnss position solution[END_REF]:

P L(α) = K(α, N 1 ) N 1 × max (eigenvalue(P ′ k|k )), (32)
where K(α, N 1 ) is obtained from a multivariate t distribution with DoF N 1 and according to the confidence level α [START_REF] Hage | Localization integrity for intelligent vehicles through fault detection and position error characterization[END_REF].

IV. EXPERIMENTAL RESULTS AND DISCUSSIONS

In this section, the approach is evaluated on data acquired with an experimental vehicle equipped with a Septentrio AsteRx SB PRO Connect for Galileo and GPS pseudorange measurements, wheel-speed sensors, a yaw-rate gyro, a Mobileye camera for lane marking detection, a NovAtel SPAN CPT for the ground truth and an HD map. Acquisitions were made at a frequency of 50Hz for dead-reckoning, 35Hz for the camera (sub-sampled at 3.5Hz to reduce time correlation) and 1Hz for GNSS measurements. The state estimation is computed at 50Hz. The vehicle was driven within the town of Compiègne for about 6 km in a suburban (to urban) area with some challenging GNSS parts as shown in figure 3. During the acquisition, the lane markings were not of high quality and mostly the roadsides were detected instead.

The error in the Along Track (AT) and Cross Track (CT) directions for the StIF before and after the FDE step are shown in figure 4. For more details about the computation of these errors, the reader can refer to [START_REF] Hage | Localization integrity for intelligent vehicles through fault detection and position error characterization[END_REF]. It can be noticed the improvement brought by the FDE step to the localization accuracy. Regarding the FDE, the residual used for the fault detection is shown in figure 5 with the associated threshold. The residuals used for the camera fault exclusion are shown in figure 6. The camera measurements give rise to multiple issues that can arise from incorrect measurement association with the HD map, poor marking quality, or faulty measurements. As for the GNSS pseudoranges, the number of excluded measurements is more limited. For instance, at the start of the trajectory, the low elevated satellite G17 is excluded (figure 7). Similarly, around the university campus (near sample 1.8 × 10 4 ), several satellites, such as E25 and G32 , from Galileo and GPS constellations respectively, are excluded due to the harsh environment and their low elevations. to the enhancement of the filter's integrity, an upper bound of 20 is imposed on the DoF. Table I presents the performance evaluation of the StIF in terms of accuracy and integrity, along with a comparison to an extended information filter (EIF). The results are provided for both cases, with and without camera fusion, while always incorporating the fusion of pseudorange measurements. The proposed StIF and EIF exhibit comparable accuracy performance. In terms of consistency and computed integrity risk, the StIF demonstrates superior performance for a T IR of 10 -3 . Without the FDE step and camera fusion, the StIF ensures integrity in the CT direction, while the EIF fails. After integrating camera measurements, both filters fail to meet the integrity requirement before the FDE step (P (error > P L) = IR > 10 -3 ). However, the StIF shows significantly better values. After the FDE step, the StIF verifies the integrity requirement in the CT direction, while the EIF does not. The importance of the FDE step for integrity and accuracy can be highlighted.

It should be noted that the improved integrity of the StIF compared to the EIF should not come at the expense of an increased uncertainty values that could diminish the availabil- The camera demonstrates a notable impact on reducing errors and PL in the CT direction. However, as this sensor is not use to detect markings in the AT direction, it does not have a great effect in that particular direction.

Based on the obtained results, it can be concluded that the end-to-end StIF with adaptive DoF is a promising approach for consistent multi-sensor data fusion using raw data. However, several points must be taken into account. Firstly, the parameters for the adaptive DoF must be carefully tuned as they have a significant impact on the performance of the filter. In our case, these values are selected based on the desired relationship between the residual and DoF. Likewise, the tuning of scale matrices of the measurements and process noises presents a similar challenge. These values are determined based on the covariances tuned for the Gaussian filter (EIF). These tuning processes require certain expertise and domain knowledge.

V. CONCLUSION

An end-to-end Student's t filter with adaptive DoF for data fusion and PL computation was proposed. To enhance localization accuracy and integrity, an FDE was added to exclude outliers. This step uses residuals based on the Kullback-Leibler divergence. The DoF is dynamically adjusted based on the quality of the observations. Experimental results demonstrate the superiority of the proposed method in achieving consistent and accurate localization using raw sensors data. Notably, the proposed method outperforms the traditional KF while maintaining non pessimistic uncertainty values. In future research, the scale matrices of measurements and process noises will be determined through a data driven approach. Additionally, the impact of non-linearity and its influence on the filter will be addressed.

Fig. 1 :

 1 Fig. 1: Frames. The camera detects a geo-referenced lane marking [AB] on the right.

Fig. 3 :

 3 Fig. 3: Compiègne suburban test and university campus.

Fig. 4 :Fig. 5 :

 45 Fig. 4: Errors before (in blue) and after (in red) the FDE step.

Figure 8 Fig. 6 :

 86 Figure 8 illustrates the adaptive DoF computed by the filter. The parameters for equation 30 are set as a = 20.114, b = -0.0791 and l = 40. To preserve heavy tails, which contribute

Fig. 7 :

 7 Fig. 7: GPS (G) and Galileo (E) elevations.

Fig. 8 :Fig. 9 :

 89 Fig. 8: Adaptation of the DoF.

  𝑿 𝒌/𝒌 , 𝑷 𝒌/𝒌 , 𝝂 𝒌 , 𝝂′ 𝒌 , 𝑷𝑳 𝒌 𝑆𝑡𝐼𝐹 1 𝑤 𝑘 ~𝑆𝑡(0, 𝑅 𝜌,𝑘 , 𝛿 𝑘 ) 𝑤 𝑘 ~𝑆𝑡(0, 𝑅 𝑐,𝑘 , 𝛿 𝑘 )

	DR Sensors	𝑣 𝑘 ~𝑆𝑡(0, 𝑄 𝑘 , 𝛾 𝑘 )		filter/ Prediction 𝑆𝑡𝐼𝐹 0 main	𝑋 𝑘/𝑘-1 , 𝑃 𝑘/𝑘-1 , 𝜈′ 𝑘-1
					+		𝑆𝑡𝐼𝐹 0 main filter/ Update
					𝑖 1 , 𝐼 1		Residual 𝑟 = 𝐾𝐿𝐷 𝑋 𝑘/𝑘 , 𝑃 𝑘/𝑘 , 𝜈 𝑘	𝜈′ 𝑘 (adapted DoF)
	pseudoranges	(GPS+Galileo)	𝜌 1 𝜌 𝑚	… 𝑆𝑡𝐼𝐹 𝑚	𝑖 𝑚 , 𝐼 𝑚	𝑟 1 𝑟 𝑚	𝑟 > Th ? Yes	No
			𝐶 0,left,1	𝑆𝑡𝐼𝐹 𝑙1		𝑟 𝑙1
	Camera	Map and HD	𝐶 0,left,2 𝐶 0,right,1	𝑆𝑡𝐼𝐹 𝑙2 𝑆𝑡𝐼𝐹 𝑟1		𝑟 𝑙2 𝑟 𝑟1	Fault detection and exclusion
			𝐶 0,right,2	𝑆𝑡𝐼𝐹 𝑟2	𝑋 𝑘/𝑘 (𝑟2) , 𝑌 𝑘/𝑘 (𝑟2) , 𝜈 𝑘 (𝑟2)	𝑟 𝑟2

TABLE I :

 I Errors and computed integrity risk (IR) (per sample) for EIF and StIF in the AT and CT directions (T IR = 10 -3 ). "StIF" refers to the proposed filter with adaptive DoF. "DoF=3" refers to StIF with fixed DoF. The best values are in bold. In dark blue, the integrity requirement is verified. ity of autonomous systems. Figure9shows the calculated PL in the AT and CT directions (T IR of 10 -3 ). Results are presented for the StIF with adapted DoF and the EIF, with camera fusion and FDE step. The StIF, while ameliorating integrity compared to the EIF, provides non-pessimistic PL with a mean values of 2.1m in the AT direction and 1.2m in the CT direction. These value are close to that of the EIF. The StIF has the advantage of increasing PL values in challenging areas. The results of a StIF with a fixed DoF=3 are also presented in table I and figure9. This filter guarantees integrity in the AT and CT directions at the expense of reduced accuracy and increased PL values. In this case, the PL mean values are approximately 5.7m in the AT direction and 4.1m in the CT direction which shows that this filter is much more pessimistic.

			Without FDE			With FDE based on the KLD
		GNSS	GNSS-camera	GNSS		GNSS-camera
		StIF	EIF	StIF	EIF	StIF	EIF	StIF	EIF	DoF=3
	absolute AT error [m]	0.69	0.68	0.63	0.62	0.68 0.68	0.61	0.6	0.78
	absolute CT error [m]	0.68	0.67	0.28	0.28	0.61 0.66	0.24 0.27	0.24
	mean absolute error [m]	1.07	1.05	0.76	0.74	1	1.04	0.71	0.72	0.86
	IR AT × 10 -3	1.3	1.6	1.1	3.9	1.3	1.6	1.5	1.8	0.02
	IR CT × 10 -3	0	10	6.8	22	0	10	0.9	15	0

The authors are with Heudiasyc laboratory, UMR 7253 CNRS, Université de Technologie de Compiègne, France. This work is part of the ANR ToICar project (ANR-21-CE33-0012) and the SIVALab joint laboratory.