ULTRASONIC BASED HEADING ESTIMATION FOR AIDING LAND VEHICLE NAVIGATION IN GNSS DENIED ENVIRONMENT

This paper introduces a novel approach for land vehicles navigation in GNSS denied environment by aiding the Inertial Navigation System (INS) with a very low-cost ultrasonic sensor using Extended Kalman Filter (EKF) to bound its drift during GNSS blockage through a heading change update to enhance the navigation estimation. The ultrasonic sensor is mounted on the body of the car facing the direction of the car motion and behind the front right wheel, a wooden surface is mounted on the car body on the other side of this wheel with a constant distance between the sensor and this surface. The ultrasonic sensor measures this range as long as the car moving straight. When orientation changes, the ultrasonic sensor senses the range to the front right wheel. The relation between the range and the estimated GNSS/INS change of heading during GNSS availability is estimated through a linear regression model. During GNSS signal outage, the ultrasonic sensor provides heading change update to the INS standalone navigation solution. Experimental road tests were performed, and the results show that the navigation states estimation using the proposed aiding is improved compared with INS standalone navigation solution during GNSS signal outage. For multiple GNSS outages of 60 seconds, the inclusion of the proposed update reduced the position RMSE to around 80% of its value when using the non-holonomic constraints and velocity update only.


INTRODUCTION
Recently, there are enormous efforts for autonomous car developments especially in the field of the navigation enhancement with low-cost navigation systems.Global Navigation Satellite System/Inertial Navigation System (GNSS/INS) integration is the most common navigation technique for land vehicles, however, when GNSS signal is blocked in some areas such as urban canyons and foliage regions, the navigation solution is degraded due to INS large drift.Therefore, GNSS/INS integration technique should be aided with other sensors to limit the INS drift such as odometer, magnetometer, Light Detection And Ranging (LIDAR), cameras, Radio Detection And Ranging (RADAR), etc...However, there are some drawbacks for using these types of sensors such as the cost, the interference, and the environmental requirements.Moreover, motion constraints such as Non-Holonomic Constraints (NHC) are used to enhance the navigation solution (Syed et al., 2008).In this research, a low-cost ultrasonic sensor is used as an aiding sensor to the INS during GNSS signal outage by providing a heading change update to the navigation system.

RELATED WORKS
Typically, the ultrasonic sensor is used in robotics (Han et al., 2009), land vehicles applications for obstacle avoidance (Borenstein and Koren, 1988), in parking assistance and in automatic collision avoidance at low speeds (Chirca et al., 2015).An autonomous vehicle was proposed by (Farooq et al., 2013) where the system consisted of GPS for positioning, two ultrasonic sensors for obstacle avoidance through Fuzzy logic technique, a camera for road lane tracking, a potentiometer for steering angle estimation, and an encoder for velocity and traveled distance measurements.In (Wangping et al., 2012), a smart car is proposed for disaster scenarios, where GPS is integrated with seven ultrasonic sensors mounted at the front for obstacle avoidance.Ultrasonic sensors were integrated with magnetometer by (Lee et al., 2008) to create an intelligent parking system for detecting vehicles inside parking regions.(Jian et al., 2008) proposed a design of a self-driving car through the integration between ultrasonic sensors and cameras for collision avoidance and lane detection purposes.(Khan and Parker, 2017) proposed an artificial neural network as a learning control for obstacle avoidance using three ultrasonic sensors placed at the front part of a car.(Lim et al., 2014) proposed a mobile robot navigation system for indoor applications, where the ultrasonic sensors are integrated with smartphone sensors (camera, accelerometers, and gyroscopes).The ultrasonic sensors are used for detecting walls and keeping a constant range from these walls while it navigates.(Moussa et al., 2018) used multiple ultrasonic system as an aiding sensor to the INS during GNSS signal outage through providing velocity updates.This system was mounted facing the left and right rear wheels to estimate the angular velocity of the rotating wheels through sensing the ranges between the ultrasonic sensor and the spokes of the wheels.

PROBLEM FORMULATION
As mentioned before, GNSS/INS integration method is the most common navigation system in land vehicles.However, there are some situations that the GNSS signal suffers from blocking, or/and multipath when the land vehicles pass through urban canyons or foliage regions.In such situations, INS only provides navigation solution which is degraded after a very short period due to the large drifts and errors of the accelerometers and the gyroscopes (Shin, 2001).
Therefore, GNSS/INS integration navigation system should be aided with other sensors to mitigate the large drift of the INS during GNSS signal outage (Niu et al., 2007).Many previous researches used various aiding sensors such as LiDAR (Gao et al., 2015), cameras (Lee et al., 2015), RADAR (Abosekeen et al., 2018), magnetometer (Won et al., 2015), and odometers.There are some disadvantages when using the previously mentioned sensors to aid INS such as some of them are expensive as the case of LiDAR and RADAR, others need very high computational and processing capability as in the case of LiDAR and cameras, others are affected by the surrounding environment as in the case of magnetic interference for the magnetometers and the weather and lighting conditions as the case of cameras and finally odometer may provide inaccurate information to the navigation system in some situations such as land vehicle slipping condition (Wang et al., 2015), or when there is some issues related to the wheels such as the wheels misalignment and unequal wheel diameters (Borenstein, 1996).
To sum up, there is no single sensor offers a practical and economical aiding navigation solution.Therefore, inclusion more sensors (preferably low-cost ones) are encouraged to reach to more economical and effective integrated navigation solution.
There are many techniques to estimate the change of heading for land vehicles such as using the Controller Area Network (CAN) bus, which provide the steering angle information from Steering Angle Sensors (SAS) (Salmon, 2015).However, this information may not be available in most of the cases as it may not be offered by most of the On-Board Diagnostic (OBD) units.Other techniques depend on using a 3D magnetometer to provide a heading update to the INS navigation solution (Abosekeen et al., 2017).However, this system may be affected by the surrounding environment due to magnetic interference.
In this paper, a low-cost ultrasonic sensor has been employed to provide a change of heading update to the INS navigation solution through Extended Kalman Filter (EKF) to mitigate the large INS drift during GNSS signal blockage.

METHODOLOGY
In this research, the ultrasonic sensor is used to aid the land vehicle navigation during GNSS denied environment by providing change of heading update to the INS through EKF.The ultrasonic sensor is mounted on the body of the car facing the direction of the car motion and behind the front right wheel, a wooden surface is mounted on the car body but on the other side of the front right wheel as shown in Figure 1 and Figure 2. Therefore, there is a constant distance between the sensor and the wooden surface which is pre-surveyed.The ultrasonic sensor measures this range as long as the car moving straight.However, when the car starts to change its orientation, the ultrasonic sensor senses the range between itself and the front right wheel.This section will be organized as follows; the first subsection discusses the ultrasonic raw data pre-processing, while the second one addresses the change of heading estimation.Finally, the last subsection discusses the integration scheme.

Ultrasonic Raw Data Pre-Processing
The ultrasonic raw data contain noise and outliers as shown in Figure 3.The first stage of the data pre-processing is the outlier removal, where the blunders are detected when the measured range lies outside the boundaries of the maximum range (between the ultrasonic sensor and the wooden plate) and the minimum range (between the sensor and the wheel when rotating to the left direction).These outliers are then eliminated from the dataset.Noise reduction is the second stage of the raw data preprocessing, where a median filter is applied to the raw data to reduce the noise.Figure 5 shows the ultrasonic processed range.

Change of Heading Estimation
During GNSS availability, the change of heading is computed using GNSS/INS integration.The relation between the ultrasonic sensor range and the change of heading is estimated through a linear regression model.Figure 6 shows the ultrasonic sensors ranges and the change of heading computed by GNSS/INS integration during the GNSS signal availability.The ultrasonic ranges can be classified into three main categories.The first category is the straight motion category where there is no rotation occurred to the front wheels.The ultrasonic ranges of this category are around 69 cm which is the range between the sensor and the wooden plate.The second category is called the right turn category where the distances between the sensor and the wheel are in the range of 42 cm to 55 cm.Finally, the left turn category, the distances are in the range of 8 cm to 21 cm.
Figure 6 shows that there are 11 right turns which are represented as small spikes, and 4 left turns which are illustrated as large spikes.There is a relation between the ultrasonic ranges and the change of heading which can be estimated through a linear regression model for each of the previous three categories.Other data set where the left turn category is dominant, is exhibited in Figure 7.The data for each category is separated from the original data to estimate a regression model for each.The regression model is implemented for half of the data for the first and second data sets where the first will form the regression model of the right turn category and the second one will create the regression model for the left turn category.Figure 8 and Figure 9 show the linear regression model for the right and left turns respectively.The Root Mean Square Error (RMSE) of the linear regression models for the right and the left turn categories are 2.59º/sec and 3.77º/sec respectively.The ultrasonic aiding system can detect the heading change for angles more than 10 degrees in case of right turns and more than 4 degrees in case of the left turns which is considered to be a drawback for this proposed system.However, installing the sensor in a different way could improve the operating range and resolution of the system.To sum up, the regression models of both the right and the left categories are estimated between the ultrasonic ranges and the GNSS/INS change of heading during GNSS signal availability.The regression model estimation scheme during the presence of GNSS signal is described in Figure 10.

Integration Scheme
GNSS/INS loosely coupled integration scheme is implemented in this research where GNSS provides position and velocity updates to INS (Noureldin et.al., 2013) through Extended Kalman Filter (EKF).EKF consists of two models: the system model and the observation (measurement) model where the system model describes the states' evolution with time while the measurement model provides the filter with the updates (Petovello, 2003).The KF system and observation models equations are described in Figure 16.
Figure 16.Kalman Filter system and observation models' equations.
For the system model: x is the time rate of change of the state vector, F is the dynamics matrix, x is the state vector, G is the shaping matrix, and w is the white noise.ϕk,k+1 is the transition matrix, I is the identity matrix and Δt is the time interval, Q is the process noise matrix which accounts for the uncertainty of the dynamic system model.For the observation model: zk is the observation, Hk is the design matrix, η is the measurement noise.R is the covariance matrix of the measurement noise which represents the uncertainty of the measurements.KF has two stages which are the prediction stage and the update stage as shown in Figure 17.Where Pk is the covariance matrix of the states, the error states δx is described as follows: where δP is the 3D position error states, δv is the 3D velocity error, δα is the attitude angles (roll, pitch, and azimuth) error states, biasa is the biases of the accelerometers, biasg is the biases of the gyroscopes.Finally, SFa and SFg are the scale factor of the accelerometers and gyroscopes respectively.The proposed integration scheme between ultrasonic aiding system and INS is described in Figure 18.
Figure 18.INS/Ultrasonic system integration scheme during GNSS signal outage The ultrasonic aiding system consists of a front ultrasonic sensor to estimate the change of heading as described before and an ultrasonic odometer to provide the filter with velocity updates as described in (Moussa et al. 2018).The ultrasonic aiding system integrates with the INS to mitigate its large drift to provide an enhanced integrated navigation solution.

EXPERIMENTAL RESULTS
Data was collected at Calgary City using Pixhawk (Px4) board which is composed of a U-blox GPS and Invensense MPU-6000 and installed on the roof of a Ford Focus car.In addition to, an ultrasonic sensor (HC-SR04) that is connected to Arduino Uno R4 embedded board.The ultrasonic sensor is mounted facing the direction of the car motion behind the front right wheel and a wooden plate which is located on the other side of that wheel, this range is kept constant for all the experimental tests.The sensor measures this fixed range when the car doesn't change its orientation.On the other side, when the right front wheel rotates to the right, the ultrasonic sensor will sense a range less than the range measured when the wheel is in straight direction, but this range is more than the range measured by the sensor when the wheel is rotated to the left direction.Two experimental data sets were collected, where the first data set is dominated by sharp right turns, while the second is dominated by sharp left turns.These datasets were used first to estimate the regression model (half of the data set) as described before in the methodology section and in providing navigation solution using the proposed navigation system.The navigation solution is estimated using loosely coupled GNSS/INS integration for all the trajectory for both data sets and then a simulated GNSS signal outage is selected on different periods to show the impact of the proposed ultrasonic aiding system on the final solution during this outage.
The RMSE for the INS standalone navigation solution was 110 meters for 60 seconds GNSS signal outage of the first data set as shown in Figure 19. Figure 20 shows that the RMSE is enhanced to around 9.70 meters when applying the proposed change of heading update along with the velocity update and NHC.The second data set is for another trajectory where it has sharp dominant left turns.The RMSE of the estimated position is around 74 meters for the INS standalone mode for 60 seconds GNSS signal outage as shown in Figure 21 while, the RMSE is improved to around 7 meters when applying the proposed change of heading update to INS as shown in Figure 22.Table 2 shows that the integration of the INS with the ultrasonic aiding system along with NHC mitigate the INS large drift where the position RMSE in enhanced to 7.11 meters instead of 74 meters for the INS standalone solution.The estimated navigation solution is improved when applying the proposed change of heading update along with the velocity update and NHC rather than applying the velocity update and NHC only.

CONCLUSIONS
A low-cost ultrasonic aiding sensor is proposed to aid the Inertial Navigation System using Extended Kalman Filter to mitigate its drift during GNSS outage through a heading change update to enhance the navigation estimation.The ultrasonic sensor is installed on the car's body facing the direction of the car motion and behind the front right wheel, a wooden surface is mounted on the car body on the other direction of this wheel.The ultrasonic senses the range to the wooden plate and the relationship between this range and the GNSS/INS estimated change of heading is determined through linear regression model during GNSS signal availability.During GNSS signal blockage, the ultrasonic ranges are used to estimate the change of heading updates through the calculated regression model.The Kalman filter is provided with this important update to limit the drift of the INS standalone navigation solution.The proposed system with other common updates such as velocity update and Non-Holonomic Constraint, enhance the position states by around 90% for 60 seconds GNSS signal blockage.The ultrasonic aiding system has many advantages such as it is a very low-cost sensor, and it has a high data rate that reaches around 92 Hertz.Finally, the proposed aiding system may be used to calibrate the gyroscopes of the INS during GNSS signal outage.

Figure 1 .
Figure 1.Ultrasonic sensor installation on the front right wheel of a Ford Focus Car.

Figure 2 .
Figure 2. Plan of the Ultrasonic sensor installation The relation between the range sensed by the ultrasonic sensor and the estimated GNSS/INS change of heading during GNSS signal availability is estimated through a linear regression model.During GNSS signal outage, the ultrasonic sensor provides heading change update to the INS navigation solution.This section will be organized as follows; the first subsection discusses the ultrasonic raw data pre-processing, while the second one addresses the change of heading estimation.Finally, the last subsection discusses the integration scheme.

Figure 3 .
Figure 3. Ultrasonic sensors raw dataTherefore, the ultrasonic sensor raw measurements are preprocessed to filter out the outliers and the noise.Figure4shows the flowchart of the ultrasonic sensors raw data pre-processing.
Figure4shows the flowchart of the ultrasonic sensors raw data pre-processing.

Figure 4 .
Figure 4. Flowchart of ultrasonic sensors raw data processing

Figure 6 .
Figure 6.Ultrasonic sensors processed data versus the change of heading from GNSS/INS integration for the dominant right turns category

Figure 7 .
Figure 7. Ultrasonic sensor processed data versus the change of heading from GNSS/INS integration for the dominant left turns category.

Figure 8 .
Figure 8. Ultrasonic sensor range versus the change of heading along with the linear regression model for the right turn category of the first data set

Figure 10 .
Figure 10.Regression Model Estimation Scheme between the ultrasonic range and the GNSS/INS change of heading during GNSS signal availability.During GNSS signal outage, the ultrasonic sensor provides heading change update to the INS standalone navigation solution.Figure 11 exhibits the INS/Ultrasonic integration scheme during the GNSS signal blockage.

Figure 11 .
Figure 11.INS/ Front Ultrasonic integration scheme during GNSS signal outage The Ultrasonic range is converted to change of heading angle through the regression model during GNSS signal outage.The heading change update aids the INS standalone navigation solution to mitigate its large drift and to provide an enhanced integrated navigation solution.Figure12and Figure13show a sample of the ultrasonic processed range along with the estimated heading change for the right and the left turns categories respectively.
Figure 12 and Figure 13 show a sample of the ultrasonic processed range along with the estimated heading change for the right and the left turns categories respectively.

Figure
Figure Ultrasonic range versus the estimated change of heading for the right turns category.

Figure 13 .
Figure 13.Ultrasonic range versus the estimated change of heading for the left turns category.There are sudden changes in the estimated change of heading because of the proposed system's resolution as described before.Therefore, a transition stage should be implemented to smoothly alter the vehicle's orientation values from straight motion to a sharp turn and vice versa.The transition stage is created using interpolation which is based on the velocity of the turns by keeping the same gradient of the estimated heading change to reconstruct the values during this stage.The estimated change of heading and the reference heading change for the right and the left turn categories are plotted in Figure14and Figure15respectively.The estimated change of heading by the proposed method is very close to the reference heading change for both the right and the left turn categories.The difference between the estimated change of heading and the reference heading change for the right and the left turn categories are calculated to evaluate the accuracy of the proposed method.The RMSE of the estimated change of heading during 60 seconds for the right turn category of the first dataset is around 1.1 º/sec while it reaches to around 1.7 º/sec for the left turn category of the second dataset.

Figure 14 .
Figure 14.The estimated change of heading versus the reference change of heading for the right turns category.

Figure 15 .
Figure 15.The estimated change of heading versus the reference change of heading for the left turns category.

Figure 17 .
Figure 17.Kalman Filter prediction and update stages equations.

Figure 19 .
Figure 19.First Trajectory INS standalone navigation solution for 60 seconds GNSS signal outage

Figure 21 .
Figure 21.Second Trajectory INS standalone navigation solution for 60 seconds GNSS signal outage

Table 1 .
Table 1 shows the average position RMSE results from different navigation solution methods for the first data set for 60 seconds GNSS signal outage.The average position RMSE for the first data set for different 60 seconds GNSS signal outages for different updates and constraints.

Table 2 .
Table 2 for the second data set for 60 seconds GNSS signal outage.The position RMSE for the second data set for 60 seconds GNSS signal outage for different updates and constraints.