Vehicle Localization Based On IMU, OBD2, and GNSS Sensor Fusion Using Extended Kalman Filter

Multiple systems have been developed to identify drivers’ drowsiness. Among all, the vehicle-based driver drowsiness detection system relies on lane lines to determine the lateral position of the vehicle for drowsiness detection. However, the lane lines may fade out, affecting its reliability. To resolve this issue, a vehicle localization algorithm based on the Inertial Measurement Unit (IMU), Global Navigation Satellite System (GNSS), and Onboard Diagnostics (OBD2) sensors is introduced. Initially, the kinematic bicycle model estimates the vehicle motion by using inputs from the OBD2 and IMU. Subsequently, the GNSS measurement is used to update the vehicle motion by applying the extended Kalman filter. To evaluate the algorithm’s performance, the tests were conducted at the residential area in Bukit Beruang, Melaka and Multimedia University Melaka Campus. The results showed that the proposed technique achieved a total root-mean-square error of 3.892 m. The extended Kalman filter also successfully reduced the drift error by 40 – 60%. Nevertheless, the extended Kalman filter suffers from the linearization error. It is recommended to employ the error-state extended Kalman filter to minimize the error. Besides, the kinematic bicycle model only generates accurate predictions at low vehicle speeds due to the assumption of zero tire slip angles. The dynamic bicycle model can be utilized to handle high-speed driving scenarios. It is also advised to integrate the LiDAR sensor since it offers supplementary position measurements, particularly in GNSS-denied environments. Lastly, the proposed technique is expected to enhance the reliability of the vehicle-based system and reduce the risk of accidents.

Extended kalman filter; Kinematic bicycle model; Vehicle localization

Out of these 3 categories of Driver Drowsiness Detection (DDD) systems, the vehicle-based measure monitors the Steering Wheel Angle (SWA), acceleration, or Standard Deviation of the Lateral Position (SDLP) to detect any abnormal driver’s conditions. A drowsy driver may demonstrate the characteristics of sluggish steering, slow change in acceleration, and frequently switching lanes while driving (Shahverdy et al., 2020; Vinckenbosch et al., 2020). Besides, the vehicle-based measure also has several limitations. First of all, it is difficult for the system to extract precise drowsiness signals (Pratama, Ardiyanto, and Adji, 2017). For example, the Lane Departure Warning System detects lane lines by incorporating a forward-looking camera behind the vehicle windshield. It cannot determine whether the vehicle has deviated from the lane if the lane lines marked on the road have faded out. Besides, the quality of the images can be easily affected by tree shadow and uneven illumination (Chen et al., 2020). To resolve the issue of low reliability of the vehicle-based DDD system, a method that can monitor the position of the vehicle in the lane without depending on the existing road infrastructure and the surrounding environment is desirable.

       The flowchart of the vehicle localization algorithm is illustrated in Figure 1. Initially, the KBM is used to estimate the vehicle motion. Two inputs are required which are the vehicle speed and the yaw rate. To acquire the vehicle speed, it is necessary to connect the Controller Area Network (CAN) bus data logger to the OBD2 connector of the vehicle. Additionally, the yaw rate can be obtained by placing the IMU at the Center of Gravity (COG) of the vehicle. Next, whenever the GNSS data is available, it will be used to update the estimated position and orientation of the vehicle by utilizing the Extended Kalman Filter (EKF). The GNSS data is obtained by using the GNSS receiver of the Android smartphone. If the GNSS data is lost when the vehicle is driving inside the tunnel, the vehicle localization will be solely dependent on the KBM.

Figure 1 The flowchart of the vehicle localization algorithm

2.1. Experimental Setup

   The vehicle localization algorithm was developed in open source language Python with all supported libraries (PySerial, Socket, and Python-CAN). The instrumented vehicle used in this experiment is Perodua Axia (SE) 2014. It has a wheelbase of 2455 mm and a trackwidth of 1410 mm. To extract the vehicle speed, the Korlan USB2CAN adapter was utilized to connect the computer to the CAN bus via the OBD2 connector. The CAN ID 0x0b0 contains information about the reading of the speedometer. It is given as the first byte of the CAN message. To decode the CAN data into the actual vehicle speed, both the CAN data and GNSS speed values were recorded at various speeds ranging from 10 to 70 km/hr, relative to the speedometer. This is shown in Table 1.

Table 1 The recorded CAN data and GNSS speed values at different speedometer readings from 10 – 70 km/hr

        From Table 1, it can be observed that the CAN data can be converted to the speedometer reading via a constant factor of 1.429. Moreover, there is a 5% difference between the speedometer reading and the GNSS speed value. This is because most automotive manufacturers will calibrate their speedometers to allow for 5% - 10% higher readings due to the requirement of traffic safety. Thus, 1.429 is divided by 1.05 to obtain the final conversion factor of 1.361. The value from the CAN bus was multiplied by this factor to retrieve the actual vehicle speed.

        Besides, the IMU was installed at the COG of the vehicle (behind the handbrake), assuming the COG is located at the center of the wheelbase and the center of the trackwidth. Additionally, IMU uses MPU-6050 (3-axis accelerometer and 3-axis gyroscope) to extract the yaw rate of the vehicle. Once the gyroscope data was received by Arduino Uno from MPU-6050, it was sent to the computer through the Bluetooth Serial Port Protocol (SPP) module HC-05. Moreover, an Android smartphone (Huawei P30) served as the GNSS receiver for measuring the vehicle's present latitude, longitude, and altitude. The Python code was executed on the Android platform using QPython 3L, a Python-integrated development environment. Once the vehicle position was acquired, it was sent to the computer through the wireless network. Finally, the vehicle speed, yaw rate, and GNSS data were collected at different sampling frequencies. For example, the Korlan USB2CAN adapter collects the vehicle speed from the CAN bus at 50 Hz (every 0.02 s) while the IMU reports the angular velocity at 10 Hz (every 0.1 s). Besides, the GNSS receiver retrieves the position of the vehicle at 1/3 Hz (every 3 s). Therefore, the resampling was performed to synchronize the time series observations. In this project, down-sampling was applied to resample the data into a 0.2 s window. The values of the data points that fell into each 0.2 s window were averaged to generate a single aggregated value.

2.2. Kinematic Bicycle Model (KBM)

        The bicycle model of the vehicle is depicted in Figure 2.

Figure 2 Kinematic Bicycle Model (Kong et al., 2015)

     In the bicycle model, both left and right wheels at the front and rear axles of the vehicle can be represented as a single wheel at points A and B respectively. The symbols  and , respectively, denote the steering angles for the front and rear wheels. The rear steering angle can be changed to zero because the model was developed under the assumption of front-wheel steering. Furthermore, point C is where the COG of the vehicle is situated. and respectively, are distances between points A and B and the COG of the car. Assuming the vehicle is having planar motion, the vehicle motion can be described by 3 state variables: represents the coordinate of the vehicle in a global (inertial) reference frame while defines the orientation of the vehicle (also known as heading angle or yaw angle). The model requires 2 inputs to fully describe the vehicle motion. The first input is the velocity at the COG of the vehicle which is denoted as in Figure 2. The velocity makes an angle with the longitudinal axis of the vehicle. This angle is known as the vehicle slip angle. Moreover, the second input of the model is the yaw rate The yaw rate is also equivalent to the angular velocity measured by the IMU about the vertical axis at the COG of the vehicle The vehicle position and orientation can be calculated by using Equations (1), (2), and (3) based on the explicit Euler method.   is referred to as the time step size.

2.3. Extended Kalman Filter (EKF)

      EKF is a powerful prediction algorithm that is used to provide estimates of some unknown variables based on a series of measurements observed over time. It is selected in this study because it is not computationally intensive and simple to implement. It consists of 2 stages: prediction and update. In the prediction stage, the EKF predicts the next state estimate by using the previous updated state estimate Once the measurement is observed, it is used to update the current state estimate, outputting  The EKF algorithm is summarized from Equations (8) until (12) where I is the identity matrix:

    From the above equations, the motion model is represented by The term is referred to as the input vector whereas is denoted as the process noise which has a (zero mean) normal distribution with a constant covariance Process noise is used to describe the uncertainty of the motion model. The equations from (1) to (7) can be rearranged into the matrix form, producing the vehicle state vector The vehicle state vector, input vector, process noise covariance matrix, and motion model are given in Equations (13), (14), (15), and (16). The terms and in Equation (15) are known as the variance of the velocity and yaw rate respectively.

On the other hand, the term  in Equation (11) represents the vehicle position measured by GNSS using the Android smartphone. The measurement model is represented The term is known as the position measurement noise of GNSS which has a (zero mean) normal distribution with a constant covariance The measurement model, together with the measurement noise covariance matrix are given in Equations (17) and (18). The terms in Equation (18) are known as the variance of  and  position measurement acquired by using the GNSS.

From Equation (9), the terms are known as the motion model Jacobians. They can be calculated via Equations (19) and (20):

Besides, from Equation (10), are called the measurement model Jacobians. They can be computed by applying Equations (21) and (22):

The data logging of x and y positions of the vehicle was conducted at 2 locations: the residential area in Bukit Beruang, Melaka (location A) and Multimedia University Melaka Campus (location B). The root-mean-square error (RMSE) of the position of the vehicle was calculated for both KBM and EKF by treating the position of the vehicle received from the GNSS as the ground truth. The results are shown in Table 2. Additionally, the paths mapped by the KBM and EKF as well as the actual paths traveled by the vehicle, collected from the GNSS are visualized on Google Maps. These are shown in Figure 3 for locations A and B.

Table 2 The Root-Mean-Square Error (RMSE) of the position of the vehicle for both KBM and EKF

Figure 3 The paths mapped by the KBM, EKF, and GNSS: (a) location A; (b) location B

Firstly, from Figure 3