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.
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 [
A large number of research works focuse on data fusion for vehicle localization applications [
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) [
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.
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.
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
The EKF passes through two main stages: Prediction and Update. The first one produces a predicted state
the command vector, P the variance/covariance confidence matrix,
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.
The prediction is made using the evolution model
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
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.
A particle i at time k is characterized by a set of attributes. The first attribute is its vector in the search space
In Equation (5), the coefficients
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
For readers interested in PSO, [
The particle filtering is a method of Sequential Monte-Carlo Simulation (SMCS) [
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
and an initial weight
Particle i has a state vector
to a normal distribution. The centered normal laws of probability used in this case are
Prediction. This stage will give an a priori ego-vehicle positioning estimate for each particle
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).
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).
Then, the particles estimates are merged to give a global ego-vehicle state estimation
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 [
In this work, the used algorithm is the systematic resampling with the Kong criterion noted in the following.
If
depending on the applied resampling approach, particles weights are set to
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
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
In the literature, approaches addressing dynamic optimization problems [XIA04] eliminate the
the basic PSO evolution Equation (5) (
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
In order to prevent the premature convergence problem, some particles are deprived of the neighborhood information
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.
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.
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
The OKPS algorithm is described in the flowchart presented in
OKPS particles, in addition to all SPF particles attributes, are fitted with a probability matrix
The parameters to be fixed are the number of particles N, the inertia weight W and the resampling factor
The initial OKPS particles attributes are:
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
A predicted vehicle state vector can be determined by the fusion of the predicted particles states vectors following the Equation (11).
Before the scores evaluation, the particle uncertainty
The updated
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 [
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.
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 [
ment [
mized and concentrated on the region maximizing the fitness values. The benefit of this evolution compared to the SPF evolution is that the
The particles results after evolution are fused in order to have a new ego-vehicle position estimation
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
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
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 (
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
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
Test AG132 | EKF | PF | SPF | OKPS | GPS |
---|---|---|---|---|---|
RMSE | 4.37 | 4.37 | 4.35 | 3.89 | 5.87 |
AEE | 4.28 | 4.27 | 4.28 | 3.76 | 5.29 |
GAE | 4.18 | 4.16 | 4.22 | 3.60 | 4.38 |
Test AG132 | EKF | PF | SPF | OKPS | GPS |
---|---|---|---|---|---|
−1.87 | 1.82 | −0.191 | −1.77 | −2.86 | |
0.49 | 0.52 | 0.37 | 0.71 | 2.48 | |
3.82 | 3.83 | 3.79 | 3.22 | 4.01 | |
0.90 | 0.92 | 0.82 | 1.06 | 2.01 |
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
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
Test AG132 | EKF | PF | SPF | OKPS | GPS |
---|---|---|---|---|---|
−1.26 | −1.29 | −1.54 | −1.30 | −6.66 | |
1.43 | 1.50 | 1.38 | 1.28 | 8.96 | |
3.76 | 3.88 | 3.89 | 3.66 | 4.82 | |
0.69 | 0.66 | 0.63 | 0.78 | 6.41 |
Test AG132 | EKF | PF | SPF | OKPS | GPS |
---|---|---|---|---|---|
RMSE | 4.28 | 4.40 | 4.45 | 4.17 | 13.72 |
AEE | 4.19 | 4.33 | 4.38 | 4.08 | 13.13 |
GAE | 4.10 | 4.25 | 4.31 | 3.98 | 12.46 |
Test AG132 | EKF | PF | SPF | OKPS | GPS |
---|---|---|---|---|---|
−1.44 | −1.43 | −0.65 | −1.12 | −7.67 | |
1.45 | 1.28 | 2.26 | 1.03 | 8.79 | |
4.18 | 4.13 | 4.56 | 2.44 | 5.43 | |
1.51 | 1.49 | 2.33 | 2.08 | 9.11 |
Test AG132 | EKF | PF | SPF | OKPS | GPS |
---|---|---|---|---|---|
RMSE | 4.89 | 4.79 | 5.63 | 3.55 | 15.73 |
AEE | 4.64 | 4.56 | 5.12 | 3.20 | 14.70 |
GAE | 4.43 | 4.37 | 4.70 | 2.91 | 13.69 |
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.
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.
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