LOCALIZATION ACCURACY IMPROVEMENT OF AUTONOMOUS VEHICLES USING SENSOR FUSION AND EXTENDED KALMAN FILTER

Advanced driver assistance systems and autonomous vehicles rely heavily on position information, therefore, enhancing localization algorithms is an actively researched ﬁeld. Novel algorithms fuse the signals of common vehicle sensors, the inertial measurement unit and global positioning system. This paper presents a localization algorithm for vehicle position estimation that integrates vehicle sensors (steering angle encoder, wheel speed sensors and a yaw-rate sensor) and GPS signals. The estimation algorithm uses an extended Kalman ﬁlter designed for a simpliﬁed version of the single track model. The vehicle dynamics-based model only includes calculation of the lateral force and planar motion of the vehicle resulting in the minimal state-space model and ﬁlter algorithm. A TESIS veDYNA vehicle dynamics and MathWorks Simulink-based simulation environment was used in the development and validation process. The presented results include different low-and high-speed maneuvers as well as ﬁlter estimates of the position and heading of the vehicle.


Introduction
Vehicle navigation systems are important components of autonomous driving solutions.These systems acquire the position, velocity and heading of the vehicle by using onboard or externally installed sensors such as wheel speed sensors, gyroscopes, accelerometers, inertial navigation systems (INS), compasses, radio frequency receivers, etc. [1].
The two most common vehicle localization techniques are dead reckoning and the use of a Global Navigation Satellite System (GNSS), like the Global Positioning System (GPS).In dead reckoning, distance and heading sensors are used to measure the vehicle displacement vector which is then integrated recursively to determine the current position of the vehicle.Measurement errors are accumulated by this integration, therefore, the accuracy of the position estimation is constantly decreasing.On the other hand, GPS provides absolute vehicle positions without the accumulation of errors associated with dead reckoning through the use of satellites as the reference points.
In this paper, a vehicle localization algorithm is presented that uses an extended Kalman filter (EKF) to integrate dead reckoning with GPS.Dead reckoning is based on a simplified version of the Single Track Model (STM) and uses a steering angle encoder, wheel speed sensors and yaw rate measurements.

Modeling Strategy
The aim of the localization algorithm is to estimate the current position (x and y coordinates) and heading (yaw angle ψ) of the vehicle by fusing GPS measurements with dead reckoning based on vehicle sensor signals.GPS measurements provide the noisy x gps and y gps coordinates.Vehicle sensors provide the steering angle δ, the four wheel speed signals ω i and the yaw rate ψ.These measurements are available in most commercial vehicles.The lateral acceleration sensors were not used because the acceleration signal is usually less reliable and noisier than the other vehicle signals.
The localization algorithm fuses these signals using an extended Kalman filter which requires an appropriate system model.The system model connects the available and estimated signals with inputs, states and outputs of the system.The simplest and most practical solution is to noise Figure 1: Dead reckoning steps and propagation of the system state formulate the system model in a way that produces the true-position and heading state variables, the input variables of vehicle sensor signals and the outputs of GPS signals.

Dead Reckoning Algorithm
Dead reckoning or path integration is the process of estimating the current position of a vehicle by using a previously determined position, and projecting that position based upon known or estimated speeds over a time period and course that has elapsed.Dead reckoning is subject to cumulative errors.In continuous time, dead reckoning results in the integration of the velocity vector or double integration of the acceleration vector with respect to time.In discrete time of sample time T S , the equivalent of integration can be written as the following in vector-sum form: The equivalent coordinate form of Eq. 1 for a vehicle in planar motion is In the vector form, r k denotes the position of the vehicle's center of gravity (COG) at t k = kT S and ∆ r k stands for the displacement between t k−1 = (k − 1) T S and t k = kT S .The initial position in vector form is r 0 and in coordinate form is [x 0 , y 0 ] T (see Fig. 1).
Dead reckoning is performed in the Earth-Fixed coordinate system X E − Y E .Using the vehicle sensors, displacements in the vehicle axis system X V − Y V can be calculated.To rotate the displacement into the Earth-Fixed coordinate system, changes in the heading or yaw angle ψ of the vehicle have to be tracked.The yaw angle can be estimated by integrating the yaw-rate signal ψ as The displacements ∆x V,k and ∆y V,k have to be calculated in the vehicle axis system X V − Y V .The displacements from the vehicle axis system to the Earth-Fixed coordinate system are transformed using a two-dimensional rotation matrix R(ψ k−1 ) that rotates around the vertical axis Z E by ψ k−1 .
The simplest estimation of the longitudinal displacement ∆x V,k involves the estimation of the longitudinal speed v k as the average of the wheel speed signals divided by the effective radius of the tires R: The lateral displacement ∆y V is estimated by the second integral of the lateral acceleration, assuming an initial lateral velocity of zero: The lateral acceleration a y can be in the form of a sensor signal from an accelerometer or calculated based Hungarian Journal of Industry and Chemistry

S r =
x y Lateral acceleration: a y Yaw rate: The physical quantities of the single track vehicle model following [11] on

Lateral Vehicle Dynamics
As a basis for modeling the lateral vehicle dynamics, the well-known single track vehicle model defined by Riekert and Schunck [12] was used.Fig. 2 shows the physical quantities related to the single track vehicle model.This paper uses the sign conventions defined in Ref. [11].
The classical single track vehicle model includes several important simplifications.It only describes lateral motion and rotation around the vertical axis, while neglects vertical dynamics as well as rolling and pitching.Furthermore, the equations of motion of the single track vehicle model are linearized.
To define the lateral acceleration as a function of the steering angle δ, longitudinal velocity v and yaw rate ψ, the lateral force equation of the model can be used: The linearized tire forces as the products of the cornering stiffnesses (c 1 and c 2 ) and tire slip angles (α 1 and α 2 ) are calculated as follows: Tire slip angles (α 1 and α 2 ) are defined by the velocity triangles of the two axles in the following nonlinear forms (see Fig. 3): Eq. 9 corresponds to the yellow part of the velocity triangle corresponding to the front axle and Eq. 10 to the rear axle.
The tire slip angle equations are linearized and the sideslip angle β omitted: The lateral acceleration a y depends on the values of the vehicle sensors according to front axle In this way, the lateral acceleration depends on the usually available cornering stiffnesses (c 1 and c 2 ), the mass and length of the vehicle (m, l 1 and l 2 ), and the signals of the vehicle sensors.If available, the lateral acceleration signal from an accelerometer can be incorporated into the sensor fusion algorithm.

Extended Kalman Filter Design
The application of an extended Kalman filter requires a stochastic mathematical model of the system in a statespace representation including the statistical properties of the process and measurement noises [13,14].

State-Space Model
The state-space representation includes both the state and measurement equations in vector form by introducing the functions f and h, the known input vector u, processnoise vector p and measurement-noise vector m: The system is time invariant, therefore, f and h are not indexed.The discrete-time indexing of the difference equations is illustrated in Fig. 1.
To estimate the vehicle's position, the dead reckoning Eqs.2-6 are used augmented to include the lateral acceleration (Eq.13) as a computed value.The resulting state equation of the discrete-time state-space model is Eq.18.
By examining Eq. 18, it can be concluded that the system is nonlinear and, therefore, a linear Kalman filter is unsuitable.In this paper, an extended Kalman filter is used.
The output vector y of the system includes the GPS measurements that consist of the true position and measurement noise m defined by the output function h:

Jacobians
The extended Kalman filter requires the Jacobians of both the state and measurement functions with respect to the state vector: The elements of H are constant, while the elements of F k depend on the discrete time k, as defined by Eq. 19.
The sample time T S was 100 ms, which was sufficiently small to capture the vehicle's movement but not too small for GPS sampling.

Noise model
During the design of the extended Kalman filter, a timeinvariant and normally distributed process as well as measurement noise were assumed: For the development and testing of our extended Kalman filter algorithm, a TESIS veDYNA vehicle dynamics-based simulation environment was used with configurable noise models (see Section 6).Although it was possible to match the noise and covariances of the filter perfectly, the filter and noise models were tuned in a slightly different way.The measurement-and processnoise covariances for both the filter and noise models have been estimated based on the characteristics of the sensor and GPS range error statistics [15]: Hungarian Journal of Industry and Chemistry Our algorithm performs the dead reckoning in the Earth-Fixed coordinate system and, therefore, no predetermined difference between the direction covariances X E and Y E is present, so Q 11 = Q 22 and R 11 = R 22 .The displacement variance of 0.2 m 2 corresponds to an error of 4.5 m s −1 in the wheel speed-based velocity calculation.
In the presence of non-normally distributed noise, the filter is not optimal.

The extended Kalman Filter Algorithm
The extended Kalman filter algorithm follows wellknown steps [14].The prediction equations are Calculation of the Kalman gain is The correction equations are The initial state is determined based on the first several GPS observations whilst in motion.

Simulation Environment
The development of the localization algorithm was conducted in a MathWorks MATLAB/Simulink-based vehicle dynamics simulation environment.The TESIS ve-DYNA model library was used to model the vehicle, road system and maneuvers (Fig. 4).The advantage of our simulation platform lies in its integrated structure.Algorithms developed in MATLAB and Simulink can be integrated into the TESIS veDYNA vehicle dynamics simulation which enables Softwarein-the-Loop testing and real-time Hardware-in-the-Loop analysis to be implemented by the National Instruments PXI hardware system.
In our platform, real streets using GPS coordinates or map databases can be modeled.This enables the direct comparison of simulations and real-world measurements.

Simulation Results
The extended Kalman filter was tuned and studied in simulated test maneuvers during which data were collected.The test maneuvers were performed on a virtual test track that included low-and high-speed driving, accelerating, braking and cornering.The size of the test track was around 2 by 2 km and the simulated test maneuvers lasted for 528 s.
Our extended Kalman filter was compared to simple dead reckoning and GPS observations.The true path, the result of dead reckoning and the position, estimated by the implementation of our extended Kalman filter, are shown in Fig. 5.

Comparison with GPS
Due to the large difference between the size of the path and the position errors, the difference between GPS observations and the extended Kalman filter estimates were not as clearly visible as the error of dead reckoning (Fig. 5).Zooming in on certain parts of the path enables visual evaluation of a particular part of the path

Real path GPS position EKF estimation
Figure 6: The initial section of the true, estimated and GPS-observed paths (Fig. 6).To evaluate the extended Kalman filter estimate and make it numerically comparable to the GPS observations, the instantaneous and average distance errors were calculated.
The distance error d k of the estimation is calculated as the distance between the estimated and true positions: The distance error d gps,k of GPS observations is The average distance errors are The distance errors of the GPS observations and the extended Kalman filter position estimates are shown in Fig. 7.The timespan of the test maneuvers was 528 s, moreover, at a sample time of 100 ms, it produced 5280 points.The average distance error of the GPS observations was dgps = 751 mm.Our extended Kalman filter reduced the average distance error significantly, d = 457 mm.
Besides the average values, the distributions of the distance errors are also of interest.The distance errors are discrete values whose distributions can be approximated by their histograms.Such histograms are shown in (Fig. 8) with a bin width of 10 cm.

Conclusions
In this paper, a real-time vehicle localization algorithm developed and implemented in a TESIS veDYNA vehicle dynamics simulation environment was presented.The localization algorithm used an extended Kalman filter to fuse GPS observations with vehicle sensor measurements of only the steering angle, yaw rate and wheel speeds.The underlying state-space model is based on planar dead reckoning that calculates the longitudinal displacement using wheel speed and lateral displacement in a simplified version of the single track model.The state-space model only includes the x and y coordinates and the yaw angle ψ to minimize the model and filter algorithm.
The performance of the position estimator was analyzed during different high-and low-speed maneuvers.Compared to the dead reckoning and GPS observations that were not integrated, the integrated system performed significantly better.The average distance error was reduced by 39 %.
Further improvement of localization accuracy would be possible by using a more sophisticated vehicle model resulting in a more complex implementation of an extended Kalman filter with a much higher computational burden.

Figure 3 :
Figure 3: Velocity triangles in the single track vehicle model

Figure 5 :
Figure 5: The real path, dead reckoning only and EKF estimation

4
Time, t, [s] Distance errors, d and d gps , [m] GPS distance error d gps EKF distance error d GPS average dgps = 751 mm EKF average d = 457 mm