Received 30 November 2015; accepted 28 December 2015; published 31 December 2015

ABSTRACT

This paper presents the Optimized Kalman Particle Swarm (OKPS) filter. This filter results from two years of research and improves the Swarm Particle Filter (SPF). The OKPS has been designed to be both cooperative and reactive. It combines the advantages of the Particle Filter (PF) and the metaheuristic Particle Swarm Optimization (PSO) for ego-vehicles localization applications. In addition to a simple fusion between the swarm optimization and the particular filtering (which leads to the Swarm Particle Filter), the OKPS uses some attributes of the Extended Kalman filter (EKF). The OKPS filter innovates by fitting its particles with a capacity of self-diagnose by means of the EKF covariance uncertainty matrix. The particles can therefore evolve by exchanging information to assess the optimized position of the ego-vehicle. The OKPS fuses data coming from embedded sensors (low cost INS, GPS and Odometer) to perform a robust ego-vehicle positioning. The OKPS is compared to the EKF filter and to filters using particles (PF and SPF) on real data from our equipped vehicle.

Keywords:

Localization, Mobile Robotic, Extended Kalman Filter, Particle Swarm Optimization, Particle Filter, Data Fusion, Vehicle Positioning, GPS

1. Introduction

Localization is a key technological component for any Advanced Driver Assistance System (ADAS). The localization information can be operated to develop new services which aim to increase drivers’ autonomy and/ or safety. These new services open fields for new Intelligent Transport Systems applied to Road applications (ITS-R) i.e. parking valet, and automated driving... Therefore, ego-vehicle accurate and reliable localization becomes an important issue in ITS research field. The objective is: to provide a much more precise vehicle positioning than with a classic Global Navigation Satellite System (GNSS) and, a reliable and consistent accu- rate positioning solution with low financial costs. Thus, on-board sensors or low cost sensors are used to comply with the GPS in order to perform a data fusion [1] . Additional sensors like inertial navigation system (INS), odometer, and steering wheel angle sensor can be used to bring new information. Fusing GPS with propriocep- tive sensors can get a better ego-vehicle positioning and at a higher rate than the GPS alone.

A large number of research works focuse on data fusion for vehicle localization applications [2] - [4] . Estimat- ing the position and orientation estimation is considered as a state estimation problem from a mathematical point of view [5] - [7] . State estimation is generally processed using Markovian methods. The Kalman Filter (KF) is an optimal filter for state estimation in the case of linear Gaussian systems [8] . Unfortunately, accurate vehicles dynamic models are nonlinear. To deal with nonlinearity, the Extended Kalman Filter (EKF) and other Kalman variants have been proposed [9] - [15] . The multi-hypothesis particular approach was developed to cover the EKF instability for the highly non-linear cases. The Particle Filter (PF) spread for research on autonomous vehicles localization and tracking applications [16] - [18] . However, the PF suffers from particles degeneracy and its accu- racy depends on the particles number which has a direct impact on the computation time [19] . Resampling prevents the particle dispersion and PF divergence [20] .

Aiming to create autonomous intelligent localization approaches for autonomous smart vehicles, the research community interests in hybrid approaches merging benefits from existing independent methods. Firstly intended for simulating social behavior, the Particle Swarm Optimization (PSO) [21] , is a metaheuristic optimization method improved for iterative optimization issues [22] - [25] . Particles of a swarm exchange information and coexist in the objective of a mutual cooperation to reach the best solution. The particles move independently toward the region where the positioning probability is higher (given the sensor information). Next, particles optimize their poses by evolving toward their best neighbors (social communication) to iteratively converge to local optima or a global optimum. Localization applications, inspired from PSO, rise within these last years [16] [17] [26] . The Swarm Particle Filter (SPF) is an evolved hybridization of a generic PF with PSO [16] [27] - [29] . PSO is used to optimize the particle distribution to overcome the PF problems of sample size dependency and particles impoverishment. The SPF can be employed for tracking and localization applications [16] [30] . How- ever, for multi-sensor data fusion based high dynamic vehicle localization, the SPF still has premature conver- gence and parameterization problems. A similar problem has been solved in [26] (where PSO is used for tuning noise parameters in addition to a Kalman filter) for dealing with strongly non-linear situations (aircraft abrupt dynamic changes). The resulting algorithm is a PSO aided EKF which needs anyway a PSO parameters tuning. We inspired from it and introduced an innovative localization method for vehicles. The Optimized Kalman Particle Swarm (OKPS) performs the vehicle real-time positioning considering a dynamic optimization problem. Earlier versions of our algorithm have been presented in [31] and [32] . This paper improves on our previous papers by introducing an adapted criteria for the driving tests and validation. Furthermore, our algorithm is fully detailed and a thorough comparison is realized with another algorithms. At each time step, the OKPS tries to find the best possible vehicle position according to the predicted vehicle state, the GPS measurement and the relayed information between particles (neighbors). The OKPS intends to minimize the parameterization needs while taking advantages of PSO/EKF/PF hybrid approach.

All these approaches are compared and ranked in terms of accuracy and robustness using real word experi- mental data of driving scenarios on the Satory-Versailles test track. Filters localization performance is evaluated in comparison with a centimetric RTK GPS used as a reference. The filters robustness is evaluated using filters uncertainty ellipses areas. The obtained results give a significant distinction between filters performances de- pending on the GPS quality (good, noisy, multi-path or missing signal).

Section 2 is dedicated to a background part; it introduces the approaches inspiring the OKPS by exposing their algorithmic and theoretical foundations. The Optimized Kalman Particle Swarm is detailed in Section 3. Section 4 carries out a theoretical and experimental filters comparison. Section 5 concludes this paper.

2. Review of Localization Methods

This section describes the approaches inspiring the OKPS filter. It gives an overview of the approaches studied for the algorithm implementation. Details of the mathematics fundamentals are given for each method.

2.1. Extended Kalman Filter (EKF)

The Extended Kalman Filter (EKF) is a recursive estimator and represents the nonlinear adaptation of the linear Kalman filter (KF). For well-defined and accurate modeling of non-linear process, EKF is the most widely used filter for state estimation. The process model must be derivable to allow the linearization by Jacobians calcula- tion. The previous estimate or state at and the current measurements are used to estimate the current state. The filtered current estimate is represented by and represents the estimation uncertainty noted as a covariance matrix.

The EKF passes through two main stages: Prediction and Update. The first one produces a predicted state using the last estimated state, the evolution model (Bicycle Model) and data from proprioceptive sensors. In the update step, the corrective measure is used to correct the predicted state. The EKF requires an initialization step giving initial values to the following parameters: X represents the state vector, U

the command vector, P the variance/covariance confidence matrix, the process noise, Q the variance/ covariance matrix representing the process noise, v: the measurement noise, R the variance/covariance matrix representing the Measurement noise and Y the measures Vector.

2.1.1. Initialization

(1)

2.1.2. Jacobians

The Jacobian matrices are matrix of partial derivatives. A and H are derived respectively from f, the transition or evolution function and h, the observation function.

(2)

2.1.3. Prediction

The prediction is made using the evolution model and its linear evolution matrix A.

(3)

2.1.4. Update

The update is a linear correction done with the Kalman gain which is calculated using the measurement matrix H to adjust the prediction according to the available measurement. The updated state is mathematically a linear weighted average between the prediction and the measurement. The weight is the Kalman gain which will tend to the prediction or the available measure according to their respective uncertainties.

(4)

2.2. Particle Swarm Optimization Basics (PSO)

The Particle Swarm Optimization (PSO) is a Metaheuristic Optimization method based on a set of samples called particles initially randomly and uniformly distributed in the search space according to a Gaussian distribution around an initial value. Each particle moves in the search space and represents a potential solution of the processed problem(s). Each particle has a memory capacity that allows it to know at each iteration what is its best performance to the current situation. A swarm particle also has the capacity of communicating with its neighbors (connected communicative particles), which allows it to know what is the best performance achieved by its neighboring particles. Using this information, each particle will move blending together three trends or tendencies: The tendency to keep its own way (selfish), the Conservative trend and the Social trend (Panurgism). For the first trend, the particle tends to use its inertia and will consequently continue keeping its own direction and evolution. By adopting the second trend, the particle tends to go back to its last best performance. For the third behavior, the particle tends to move toward the best solution found by its neighborhood. Figure 1 shows the PSO particle motion principle illustrating the tendencies collaborative mechanism of the particles.

A particle i at time k is characterized by a set of attributes. The first attribute is its vector in the search space. The second one is the displacement information which can be taken and is noted as a speed. Then, the third one is a set of performance information. The first performance data corresponds to the best solution found by a particle (Personal Best). The second performance data characterizes the best solution at iteration k reached by all neighbors (Global Best). At each new iteration, the position of the particle i is updated using the previously mentioned information applying the PSO motion principle described in the following evolution equations:

(5)

In Equation (5), the coefficients and are learning terms. The coefficients and are random numbers following a uniform distribution. These terms are used to produce a variation in the development of each particle providing a diversity of attitude that supports the search space exploration. The coefficient W is the inertia factor. Some variants of these evolution equations have been proposed in various studies [16] [21] [33] [34] . Each of these variants of evolution equations is designed to balance between the tendency to explore and the tendency to converge towards the optimal solution (swarm condensation/dispersion). For more details, [35] and [36] propose comparative studies of these PSO evolution variants. For the particles interaction, it exists two principal sorts of neighborhood topologies (geographical dynamic neighborhood and static social neighborhood). The more the neighborhood is informative the more information is shared supporting the swarm convergence.

The geographical neighborhood topology is based on the nearby particles and is considered as a dynamic topology because it needs to be calculated at each iteration (after particles evolution). The social neighborhood configurations shown in Figure 2 are set at the beginning of the process and do not require distance calculation to find the neighbors. Note that generally, in case of particles convergence, the social neighborhood tends to become geographical.

For readers interested in PSO, [35] and [37] provides an interesting analysis of recent developments in PSO evolution strategies and neighborhood topologies providing the basic algorithm and most of its variants. The details of the followed steps in the basic Particle Swarm Optimization algorithm are described in Algorithm 1.

2.3. Particle Filter (PF)

The particle filtering is a method of Sequential Monte-Carlo Simulation (SMCS) [19] . Using empirical distribu- tion of particles, the Particle Filter (PF) approximates the distribution of an estimated process. Particles explore the state space evolving independently in the search space. Each particle is a possible state of the process. Particles dispersion and concentration are managed by an Importance Sampling-Resampling (ISR) mechanism [20] . The sampling attributes a weight to each particle representing its actual consistency. The resampling up- dates these weights and performs a duplication-elimination mechanism according to the updated attributed weights. These two importance selection mechanisms work together to guarantee the particles distribution homogeneity and the PF stability avoiding the particles impoverishment. The PF is a good alternative to the non- linear Kalman filter variants for the ego-vehicle positioning application. With an adequate number of particles, the PF approximates the optimal probability distribution of an estimate. In comparison with the Kalman filters, the PF can be more accurate and more robust in case of strong non-linearity and noises. However, the PF

performance depends directly on the available computation power (number of samples). The user must do a compromise between computational time and accuracy. The particle filter algorithmic steps (see Algorithm 2) are detailed in the following:

Initialization. The initial position of the ego-vehicle is represented by the state vector. The initial cloud of particles is drawn around the initial position by assigning each particle a state vector

and an initial weight. N is the number of particles (samples) which is to determined by the user. The state vectors of the particles represent a Gaussian distribution centered on the initial position.

Particle i has a state vector and a weight.

are random variables representing the uncertainty of the initial positioning state. The uncertainty ellipse will be determined by the particles scattering across the search space around the initial position according

to a normal distribution. The centered normal laws of probability used in this case are, ,. When, are the initial standard deviations noise values given by the sensors charac- teristics and/or the standard deviation of an initialization data set.

Prediction. This stage will give an a priori ego-vehicle positioning estimate for each particle. Using the proprioceptive sensors data and the last localization estimate, the particles will predict the vehicle state. This predicted position is calculated using the bicycle vehicle model integrating the proprioceptive sensors information and noises. The prediction represents the vehicle possible actual state according to

the last known state and the evolutionary information until this moment. Uncertainties of the evolution model and those of proprioceptive sensors data are very important for the proper functioning of the PF filter. These uncertainties are incorporated in order to promote the diversity in the particles evolution. Giving a different prediction evolution for each particle, this integration of the measurements and modeling uncertainties is the main reason which allows the particles to better explore the search space. When these noises are too low, the filter will not explore the search space and noise conditions will not be well represented by the particles distribution. When the noises are too high, the filter suffers from particles impoverishment due to excessive scattering of particles on several consecutive steps.

Update. In this stage, the prediction of each particle is adjusted by the reassessment of particles weights according to a new updated exteroceptive sensor data provided for instance by a low cost GPS. The update can be taken as compromising between the predictive and the corrective estimates. The conciliation between these two estimates is done by taking into account their respective uncertainties. The calculation of particles weight is carried out by Equation (6).

(6)

Standardization and Estimation. The weights standardization of the particles is done after updating to normalize the probabilities sum. The standardized weights are calculated according to the Equation (7).

(7)

Then, the particles estimates are merged to give a global ego-vehicle state estimation. The vehicle position can be calculated with the Equation (8):

(8)

Resampling. Resampling is a critical step for the stability of the particle filter. It is a selective mechanism that eliminates low weight particles and duplicates particles with high weights. The aim of applying the resampl- ing mechanism is to control the particles dispersion without affecting the probabilistic distribution of the particles (minimum impact on the ellipse of uncertainty).

It exists a wide range of methods for resampling as well as criteria for enabling/disabling resampling. For more details, see here [20] .

In this work, the used algorithm is the systematic resampling with the Kong criterion noted in the following.

(9)

is the Kong criterion which indicates the number of effective particles according to their standardized weights. tends to N when the distribution of the particles weights is efficient and tends to 1 when the distribution of the particles is inefficient.

If is under the defined threshold, the resampling is then performed. The threshold represents the minimum of required effectiveness, it is generally a fixed value within this range. It is fixed using the term. with N the number of particles and represents the minimum desired percentage of consistent particles, for example for 50% of minimum effective particles. Note that

depending on the applied resampling approach, particles weights are set to or recalculated depending on particles duplication number after the resampling process.

2.4. The Swarm Particle Filter (SPF)

The particles of the PF do not perform any individual correction when the GPS is available. The estimation in

the PF is done only using the weighted predictions, see Equation (8). In order to propose an evolutionary

approach which performs a real corrective step in the updating stage, the Swarm Particle Filter was developed.

Inspired by the PSO, the SPF is a hybridization of the PF with an integration of the social influence of the PSO. The SPF is expected to bring a particles interaction providing the capability for each particle to evolve in function of the neighborhood (correct the prediction). Particles go through all the normal PF steps. In addition to the classic PF stages, after the update stage, particles communicate and evolve in order to optimize their estimate and the swarm distribution. The particles move toward the region maximizing the positioning probabilities (particles weights).

The SPF performs in addition to the PF steps an evolution step just after the update and before the ego-vehicle positioning calculation and resampling. The SPF evolution step is the same concept of the evolution in PSO. However, the evolution equations are adapted to the application, giving the Equation (10). The scores are calculated as PF weights and is obtained by comparing the particles weights. Figure 3 describes the PF hybridization giving the SPF algorithm (see Algorithm 3).

(10)

In the literature, approaches addressing dynamic optimization problems [XIA04] eliminate the part from

the basic PSO evolution Equation (5) (is set equal to the particle current position), this operation is called memory erasing. By considering the optimization in a dynamic environment, the choice of erasing memories of the particles is adopted. The PSO Equation (5) gives then the SPF evolution Equation (10). This evolution variant also integrates Gaussian random numbers instead of learning factor with uniform random numbers. It was proposed and validated by [34] with the advantage of minimizing the number of parameters to be fixed in comparison with the basic PSO evolution.

The SPF filter applied to the ego-vehicle localization showed problems of premature convergence. The main reason is that the particles exchange only weight information. This weight calculated using Equation (6) con- siders the particle performance exclusively according to the GPS data. Thus, when the GPS data is erroneous and its measurement error matrix is not sufficiently representative of the measure noise, the particles are attracted by the GPS data. By applying this principle, the SPF develops after few localization steps too much dependency on GPS data and the particles distribution is concentrated steeply around the current GPS data. This behavior penalizes the filter in the next prediction steps until a new GPS data arrives. Complying with the PF filtering and PSO evolution concept, The SPF suffers generally from premature convergence or swarm explo- sion problems. The SPF needs also PSO parameters tuning by the user.

In order to prevent the premature convergence problem, some particles are deprived of the neighborhood information which creates troublemaking particles preventing premature swarm concentration. The trou-

blemaking particles (blind/non evolutive particles) are selected by fixing their number at the beginning or chose randomly at each step. These kind of particles do not apply the PSO optimization encouraging the swarm expansion and the search space exploration.

Based on the SPF concept and trying to overcome the problems of the GPS attraction and premature conver- gence, the OKPS filter was developed to perform a reactive-cooperative ego-vehicle localization. The idea of the OKPS conception is to allow each particle to judge its uncertainty not only relatively to GPS data, but by taking also into account the swarm dynamic (theoretically the vehicle dynamic). It will make each particle acting as a sensor fitted with self-diagnostic capacity. Then, each particle will provide an estimate and its associated estima- tion error.

3. The Optimized Kalman Particle Swarm (OKPS)

The OKPS is an evolution of the SPF. This proposed filter integrates in addition to the social concept of the SPF a cognitive one. The cognitive concept consists in giving an intelligence self-diagnostic capacity to the swarm particles. An adaptive weighting function, called fitness function is also developed to allow adaptive particles weights calculation considering this new capacity. The fitness function takes the EKF gain concept in order to be as representative as possible of the particle current probability. The role of the fitness function is to give a weight for each particle considering its efficiency relatively to the GPS measure and relatively to the particle confidence on its prediction at the same time.

3.1. OKPS Implementation

To perform an optimization-filtering approach allowing to be both reactive and cooperative, the OKPS combines the advantages of the already presented techniques. The cooperative aspect consists in the information exchanging and interaction between particles. While, the reactive aspect comes out in the capacity of detecting changes in the vehicle’s dynamic. This capacity is the direct consequence of fitting particles with a simple self-diagnose mechanism. The idea is performed by enhancing particles with a probability matrix (Inspired from the uncertainty matrix P of the EKF method) allowing each particle to evaluate its likelihood. The matrix has to be as representative as possible of the particle positioning uncertainty. It is the reason why this matrix is managed (updated at each step) by an Extended Kalman filter gain K. After an initialization step, the OKPS filter performs the localization following the logical step of prediction and update integrating the new particles and swarm features. The OKPS algorithm (Algorithm 4) can be considered as an evolved SPF hybrid approach aided by a Kalman gain to reassess the particles likelihood used in an adaptive fitness function.

The OKPS algorithm is described in the flowchart presented in Figure 4 and the implementation details are given in the following:

3.1.1. Initialization

OKPS particles, in addition to all SPF particles attributes, are fitted with a probability matrix. This matrix is initialized as an EKF variance confidence matrix.

The parameters to be fixed are the number of particles N, the inertia weight W and the resampling factor. The initial collected data are used to define the initial position, calculated with an initial sensors data set giving, , and values (calculation described in Equation (1)). The swarm is initialized following the initialization formulation noted in 2.3.

The initial OKPS particles attributes are: which represents a positioning vector, represents the initial value of the particle speed of evolution, the particle initial uncertainty matrix. The particles initial (weights) fitness score value is noted, it will be updated for every avail- able GPS data using Equation (13), and gives the global best among neighborhood initial solutions.

3.1.2. Prediction

Particles predict the vehicle state using proprioceptive data and the bicycle vehicle model. In addition to the state prediction, a prediction likelihood is calculated with according to the EKF prediction Equation (3).

A predicted vehicle state vector can be determined by the fusion of the predicted particles states vectors following the Equation (11).

(11)

3.1.3. Updating Scores

Before the scores evaluation, the particle uncertainty and the measure uncertainty matrices are up- dated. The first one is updated using a Kalman gain to be as representative as possible of the particle i uncer- tainty, see Equation (12). The second uncertainty matrix is updated by the GPS quality data acquisition.

The updated is made according to Equation (12).

(12)

Then, the score for each particle is calculated with the Fitness function. The Fitness function is a minimization or maximization criterion representing one or a compromise of multiple goals, the selection of this function depends on the application and the desired result [22] [25] [27] .

The OKPS fitness Function (13) is a maximization criterion. The calculated fitness score considers two information sources: the GPS corrective source and the particle prediction source. The compromise between these two sources will be done by a weighted average of their respective quadratic errors relatively to their respective uncertainties.

(13)

is the observation (predicted measure). is the GPS measure, represents the GPS uncertainty and the updated estimation uncertainty. is the predicted vehicle state and The is determined by comparing the particles scores. The best particle is the particle with highest score after scores normalization.

3.1.4. Evolution

In this step, each particle will optimize its estimation by evolving in the search space. This evolution is done by following Equation (10) and applying the Gaussian PSO motion principle [34] adapted to a dynamic environ-

ment [35] . The particle merges its inertia and its attraction to the in order to move toward a region of interest. This will give a new updated position of each particle. Thus, the swarm distribution will be opti-

mized and concentrated on the region maximizing the fitness values. The benefit of this evolution compared to the SPF evolution is that the is defined with an adaptive fitness function. Instead of SPF particles weight- ing which is only relative to the GPS position, this new adaptive weighting allows inertial behavior avoiding particles sticking to GPS. This adaptive evolution make the filter more robust to GPS outliers giving more smooth positioning.

3.1.5. Estimation

The particles results after evolution are fused in order to have a new ego-vehicle position estimation. Particles estimations are fused using standardized weights which are the updated standardized fitness scores. The fusion for the global estimation is made with the following equation.

3.1.6. Resampling

The resampling algorithm and criterion are the same used for the previously described approaches, the algorithm is the systematic one and the criterion is the Kong effective particles number.

In the same conditions of resampling threshold and processed data, the resampling triggering for OKPS occurs less than for the SPF because of the adaptive optimization that makes the particles more reactive and con- sequently rises the number of effective particles. The resampling process is still a heavy computational process and avoiding useless resampling is advantageous. Moreover, the less the particle population is resampled the more the estimation dynamic characteristics are preserved.

4. Results Analysis

In this section, the filters: EKF, PF, SPF and OKPS are tested in an ego-vehicle localization application. Different scenarios with different data qualities and noises conditions are studied. The filters will be ranked in terms of accuracy and robustness. The main criteria of comparison are: the Root Mean Square Error (RMSE), the Average Euclidean Error (AEE) and the Geometric Average Error (GAE) for the category of average errors. The RMSE is the mostly used natural approximation of the mean standard deviation of the differences between predicted values and observed values (estimation error). RMSE represents a good measure of accuracy. However, as it has not a simple physical interpretation and is scale-dependent. It is generally used for proba- bilistic analysis. Taken from the Euclidean distance concept, the AEE represents the average of instantaneous

errors while the RMSE is an approximation of the standard error. The

RMSE and AEE are analysis criteria of average errors based on the concept of the arithmetic mean. The dis- advantage of this concept is that it is very sensitive to large outliers. To overcome this criteria weakness, the Geometric Average Error (GAE) is more robust to large outliers. Theoretically, the GAE is never greater than the AEE which is never even higher than the RMSE value (). Thus, the impact of impor- tant errors is larger on RMSE and smaller on GAE. The main interest of using GAE is to be less influenced by the large errors values and remaining close to the AEE value. only when the number of samples is limited to one. Moreover, the gap between the GAE, AEE and RMSE can be used to detect the presence of temporary high outliers. The more this three average criteria are close the more the mistakes were homogeneous (no error peaks). When, there has been no errors fluctuation and the error can be con- sidered as a bias. Then, this bias can be counterbalanced.

In order to evaluate the instantaneous filters performance, complementary criteria to the average errors are taken into consideration, such as the instantaneous Euclidean Error (EE), axial errors, mean axial errors and mean axial standard deviations. To also evaluate filters robustness and sensitivity, the relationship between the average and instantaneous criteria is used.

To perform an even-handed experimental comparison, real word data are collected during a driving scenario on the urban area of the Satory test track (see Figure 5). Data collecting is done using a vehicle of the IFSTTAR/LIVIC laboratory equipped with embedded sensors and dedicated software. The vehicle is equipped with a steering wheel angle encoder, a gyrometer, an odometer and a low cost AG132 GPS running in degraded mode. Using a real data base is the best way to guarantee a real positioning tests (Data, conditions and noise similarity). The tests are then stated in the same conditions with the same data and with the same computational resources. For multi-hypothesis approaches (particular methods), the particles number is fixed to 500. The effectiveness threshold for particles resampling is set to a minimum of 50% of effective particles. As said before, to avoid the SPF divergence caused by the swarm concentration around the GPS data, a percentage of communicative particles is attributed to this filter. The communicative-evolutionary SPF particles will then represent 10% of the swarm. For OKPS and SPF evolving particles, the inertia weight W is set to 0.2 to guarantee a minimum consideration of the vehicle inertia. The reference for positioning errors calculation is the centimetric differential GPS RTK. Tests are carried out in the same scenario with three different conditions. The vehicle is driven from right to left along the course delimited by flags on the Satory test track (Figure 5).

The filters performance shown by axial errors and Euclidean error graphics describe the filters instantaneous behavior at each step of the test. The tables of mean axial errors and standard deviations resume the global performance by mean values giving an idea of the average performance. The accuracy will be stated by analyzing the average errors and the robustness will be an analysis of the axial standard deviation relation with axial errors.

The first test is done using the synchronized data base. As data are synchronized, the data of slower sensors are interpolated. The localization process will perform both prediction and correction at each time step. This kind of situation are possible when the sensors are fast or when the vehicle moves slowly (parking maneuver). The problem with this data base is that the full data availability makes the filters very confident and optimist. The uncertainty ellipses and values will be smaller than normal and the challenge is to be capable of detecting outliers in spite of the confidence in the measurement. This data base is characterized by a bias and a high level of noise caused by the synchronization step. The filters have to be robust and precise at the same time. As said before, the difficulty is to filter outliers and not to be fully optimistic about data quality.

The results of the AG132 synchronous test are shown in Figure 6 and Table 1 and Table 2. Figure 6 describes according to the subfigures from left to right and from top to bottom the following criteria: the Euclidean Error, the RMSE, the AEE, the GAE, the longitudinal X axial error and the lateral Y axial error. Table 2 gives the axial mean errors for each filter in comparison with the GPS one and gives also the axial standard deviations. Table 1 compares the average errors rating the errors fluctuation using the gap between the GAE, AEE and RMSE. As noted in Table 1 the gap between the average errors criteria is significant which supports mathematically the presence of fluctuations (presence of temporary high outliers). The variations are also visible in the GPS Euclidean error results. The global results show that all the filters correct the errors and perform an acceptable ego-vehicle positioning. However, the filters remain more attracted by the biased GPS data. Here the OKPS outperforms the other filters by always keeping particles reactive and cooperative. To achieve this result, the OKPS gives an adequate likelihood value to each particle at each time step using the adaptive fitness function. Then, these values of particles likelihood (scores) are used to give a global estimation value for the ego-vehicle localization. The OKPS is finally more accurate and more robust to strong noises and bias. The filters axial standard deviations noted in Table 2 show that the OKPS did not become more optimistic than necessary which allows it to filter more effectively GPS outliers. According to the axial mean errors and standard deviation relationship the OKPS is 11.7% more accurate than the EKF and 11.4% than the PF, the OKPS is also 7.8% more robust than the EKF and 6.8% less sensitive than the PF.

The OKPS merges both information from prediction and correction, the adaptive weighting mechanism (fitness) and the particles self-diagnose (uncertainty matrix) allow to get the best estimation of the vehicle position. However, these results which are obtained by a GPS information penalization because of their strongly degraded and noisy character, seems to hide an OKPS divergence.

To test our filter consistency and to ensure its integrity, further tests are carried out. First, the sensors data will be taken in their raw version (no signal pre-processing and no synchronization). This test will give an idea about the approaches performance in a localization application for a standard vehicle equipped with low cost sensors. After that, a GPS disturbance is generated and included to the GPS measures. The generated GPS degradation simulates the effect of GPS multi-reflexion in urban canyons. This final test will test the ego-vehicle localization performance for an urban driving scenario.

The results of the second test done with the raw asynchronous GPS data are shown in Figure 7, Table 3 and Table 4. The graphics shows that the OKPS is still performing good ego-vehicle localization in normal signal conditions. This result ensures the OKPS efficiency for different scenarios and supports the OKPS integrity. From axial errors analysis and standard deviations, we can say that the OKPS is 3.6% more accurate than the SPF and 1.2% more robust than the PF. For this test, the SPF an PF particles have the same behavior, the SPF outperforms the PF because of the SPF evolutionary capacity (optimization) which is not possible for the PF particles.

In order to test the filters reactivity and sensitivity in another situation, this third and last test is carried out. The results of the urban canyon driving scenario are synthesized in Figure 8, Table 5 and Table 6. The OKPS is designed to deal with this kind of situations, that is why it performs the best localization results. In this test, the GPS data are punctually disturbed by a multipath noise. These disturbances are shown by the peaks in graphics, especially in instantaneous criteria such as Euclidean Error and axial errors. The filters estimations are almost influenced by the peaks but the OKPS remains the less sensitive one. In a city with urban canyons where GPS systems generally suffers from multipath, multireflexion and outage problems, the OKPS will be the most appropriate localization approach. It is 18.4% more accurate than the EKF and 17.9% more than the PF. It is also 7.3% more robust than the SPF to multipath. The gap between RMSE, AEE, and GAE is more important than for the two previous tests which confirms the presence of important punctual disturbances.

The OKPS outcompetes the other approaches especially in signal multireflexion cases. It also remains less sensitive to the GPS positioning outliers and vehicle dynamic changes than the EKF, PF and SPF filters. The OKPS performs a better positioning with a higher accuracy in different signal and driving situations. These tests conclude that the OKPS is better overall the three scenarios and overtakes the other filters especially in case of GPS multipaths and sensors data disturbance.

5. Conclusions

This paper shows the Optimized Kalman Particle Swarm theoretical formulation and experimental performance.

It highlights the cooperative reactive aspect of the OKPS which performs accurate ego-vehicle localization in degraded conditions (noises and multireflexions). Our OKPS fits the particles with an uncertainty matrix. The covariance uncertainty matrix represents the capacity of auto-diagnose of the particles which are incorporated to the adaptive weighting system (fitness function). Thanks to the added covariance matrix, the particles of the OKPS become more reactive to abrupt dynamic changes and more robust to noises.

The advantage of the OKPS positioning is stated during a driving scenario test. The OKPS outperforms the other filters using its reactivity and cooperative particles. More explicitly, the adaptive multi-objective fitness function allows the swarm to evolve to high scores regions. Each particle merges its self-diagnose with the GPS data. The described cooperative process makes the OKPS effective in high dynamic on-road ego-vehicle locali- zation applications. The OKPS performs the best ego-vehicle positioning especially for the urban driving scena- rio with GPS multipaths: it out-competes the EKF, PF and SPF for ego-vehicle localization application. Even though the OKPS is more computationally complex and more time consuming, its promising results make it one of the most suitable localization methods. The OKPS needs less tuning parameters than the metaheuristic hybrid localization approaches. An auto-attraction-repulsion mechanism insures the swarm homogeneity, diversifica- tion and effectiveness. This mechanism prevents the swarm premature convergence for a full connected neigh- borhood topology.

In future works, the OKPS will be tested in more diverse driving scenarios (for example stop and go, parking and strong braking and acceleration scenarios) with additional sensors which will improve the OKPS localiza- tion accuracy and integrity. This approach will then be tested in comparison with the Interacting Multi-Model Filter developed by the LIVIC laboratory for autonomous vehicle localization applications. Next, we intend to add nice properties of the IMM in the OKPS.

Cite this paper

Adda Redouane AhmedBacha,DominiqueGruyer,AlainLambert, (2016) OKPS: A Reactive/Cooperative Multi-Sensors Data Fusion Approach Designed for Robust Vehicle Localization. Positioning,07,1-20. doi: 10.4236/pos.2016.71001

References