RADAR/INS INTEGRATION FOR POSE ESTIMATION IN GNSS-DENIED ENVIRONMENTS

This paper proposes a novel algorithm to use Radar in ego-motion estimation for autonomous navigation applications. This method is based on the analysis of Radar data to remove noise, ghost points, and outliers and keep the accurate features. From the detected features and the knowledge of Radar data rate and the vehicle's average speed, the change in range and azimuth between any two points can be constrained to find the corresponding points. With the help of the corresponding points, the vehicle's ego-motion can be estimated. Then, Radar is integrated with an Inertial Navigation System (INS) and odometer through an extended Kalman filter (EKF) to smooth the Radar solution and aid INS to overcome its large drifts in GNSS denied environments. Two real data were collected from frequency modulated continuous wave (FMCW) Radar sensors and Inertial Measurement Unit (IMU) in suburban areas near the University of Calgary, Canada. The proposed algorithm was tested by introducing simulated GNSS signal outages with different durations. The Root Mean Square Error (RMSE) for the horizontal position was improved by an average of 30.44% and 4.76% if it was compared with RMSE from odometer/INS solution with a percentage error less than 1% of the traveled distance which was 1.59 km and 2 km for the two datasets, respectively.


INTRODUCTION
Autonomous vehicles consist of multiple components. The first and the most crucial component is autonomous navigation because the other components depend on the vehicle's navigation state and its accuracy. The major challenge in autonomous navigation has been to provide a reliable and continuous solution in all environments. GNSS is the most used navigation sensor in autonomous vehicles applications as it can estimate the vehicle's position, velocity, and heading with high accuracy. However, GNSS cannot be relied upon in some scenarios, especially GNSS challenging environments, such as, in underground parking, through canyons, or beside tall buildings, where the GNSS signal is partially or fully blocked (Elsheikh & Noureldin, 2020;Li, 2021, Liu et al., 2010) An Inertial Navigation System (INS) is a dead reckoning (DR) system that can estimate the relative position and relative attitude. INS has the advantage of working indoors and outdoors, solving the continuity problem. Moreover, it works with a high data rate; nevertheless, the INS navigation state cannot be used as a standalone solution since INS navigation solution deteriorates and drifts with time (Wang et al., 2017). Thus, INS is, normally, integrated with GNSS to compensate for GNSS signal outages and overcome the INS errors. There are three types of INS/GNSS integration: loosely coupled, tightly coupled, and deeply coupled (Noureldin et al., 2013). However, in case of the GNSS signal outage for a long time, INS needs to be aided by other sensors to overcome the INS drift. The magnetometer and odometer are two examples of the sensors used to aid INS. However, the magnetometer is affected by the surrounding magnetic field (Renaudin et al., 2010). Recently, Camera and light detection and range (Lidar) have been used to aid INS and in other autonomous navigation * Corresponding author applications, e.g., Simultaneous Localization And Mapping (SLAM) (Kim et al., 2004;Travis et al., 2005). However, vision sensors cannot work in different light and weather conditions (Bila et al., 2017;Huang & Liu, 2018). Lidar also is affected by fog and rainy weather conditions (Yoneda et al., 2018). Moreover, it requires a high computational power due to its dense point clouds. Lidar also is a high-cost sensor. Radio detection and range (Radar) is an all-weather sensor since it can work in different weather and light conditions. So, INS and Radar are the only two sensors that can solve the continuity problem. Therefore, this paper focuses on integrating INS and Radar to estimate the vehicle's ego-motion in GNSS denied environments. This paper is organized as follows: Section 2 contains the related work, while the methodology is described in section 3. In section 4, the experimental work and results are discussed, and finally, the conclusion is shown in section 5.

RELATED WORK
As mentioned in the previous section, Radar can work in different environments. FMCW Radar transmits a Radio wave that reflects from the surrounding objects. Thus, the range from the Radar unit to these objects can be determined based on the time of flight (TOF). The relative velocity between the vehicle containing the Radar unit and the surrounding objects can be estimated from the doppler frequency shift. Thus, Radar is used in Adaptive Cruise Control (ACC) to avoid obstacles, detect the pedestrians, range, and velocity of the surrounding vehicles (de Ponte Müller, 2017). Besides, Radar is used in autonomous navigation to estimate the vehicle's pose. The Iterative Closest Point (ICP) is the most common technique to estimate the transition and rotation between point clouds in any two successive Radar frames (Censi, 2008). ICP is based on minimizing the distance between the corresponding points to estimate the vehicle's ego-motion. However, this technique is affected by outliers and could need more iterations to estimate the vehicle's pose (Nießner et al., 2014). ICP was improved by using the Inertial Measurement Unit (IMU) to estimate an initial rotation to reduce the iterations needed in ICP (Xu et al., 2018). Normal Distribution Transform (NDT) differs from ICP as it is based on building a normal distribution model for the point clouds in each Radar frame. Then, the models are matched to estimate the vehicle's rotation and transition. Compared with ICP, NDT is not affected by outliers (Biber & Strasser, 2003). Fourier-Mellin Transform (FMT) is another technique used in autonomous navigation. FMT converts the Radar frame into the Fourier domain. The rotation and transition between Radar frames are converted as a linear shift (Gérossier et al., 2009). Visual feature detection methods, e.g., SIFT and ORB, were exploited in autonomous vehicles. (Callmer et al., 2011) fixed a 360 o Radar on a vessel. SIFT was adopted to detect and extract the features from Radar images and build the descriptor for each feature. The features were matched based on the descriptor. Then, the rotation and transition can be estimated. (Elkholy et al., 2021) used ORB for autonomous vehicle applications to detect the features and estimate the rotation and transition between Radar frames. ORB was proved to be more efficient than the traditional scan matching techniques, e.g., ICP or NDT, and other visual feature detection methods, e.g., SIFT and SURF. Constant False Alarm Rate (CFAR) is another method that can detect the features by applying a moving filter to remove the noise and outliers and keep the real features (Rohling, 2011). (Cen & Newman, 2018) used a 360 o FMCW Radar for vehicle motion estimation. They applied a median filter, binominal filter, and threshold to remove the noise and outliers and detect the landmarks from Radar data. The authors used the unary descriptor and the relationships between the landmarks (Euclidian distance) for data association. The relative motion was estimated by applying singular value decomposition (SVD) (Challis, 1995). Radar also was integrated with a Reduced Inertial Sensor System (RISS) to enhance the navigation solution and limit the INS errors. The RISS is used as an alternative sensor to the traditional INS as it uses fewer sensors than the regular INS (Iqbal et al., 2008). (Rashed et al., 2019) integrated FMCW Radar with RISS through Kalman Filter for land vehicle navigation applications. The Radar unit was mounted on the front bomber. So, the relative distance between the vehicle and the in-front vehicle can be estimated, and the traveled distance can be estimated based on that. However, this method might fail if there are no vehicles in front. (Al-Qudsi et al., 2014) integrated INS with FMCW Radar using EKF for indoor applications. A particle filter was applied to estimate the position from Radar based on the time difference of arrival. (Abosekeen et al., 2018) integrated Radar with RISS through EKF. FMCW Radar unit was mounted at the front bomber facing the ground. Thus, the estimated relative velocity from the reflected signals refers to the forward vehicle velocity. The authors improved the horizontal position RMSE during 50 seconds of GNSS outage from 47.57 m to 7.14 m. (Almalioglu et al., 2021) integrated IMU sensor with Millimeter-wave (MMwave) Radar through Unscented Kalman Filter (UKF) for indoor ego-motion estimation. The authors adopted NDT with angular velocity from the IMU sensor to eject errors from IMU such as biases and remove Radar noise. This paper applies a new method for Radar data association and develops a new algorithm for Radar ego-motion estimation. Also, Radar/odometer/INS integration is adopted through a closed-loop EKF. Therefore, INS can smooth Radar solution, and Radar can overcome INS limitations and errors and compensate for GNSS signal outage during different periods up to 4 minutes outage.

METHODOLOGY
Radar data is noisy because Radar is affected by wave multipath which can create ghost points and interact with other Radar waves in the surrounding environment. Therefore, the first step in Radar data processing is analyzing and preprocessing to remove all the outliers, noise, and false objects. After removing the outliers, the next step is data association, in which the detected objects are matched between the successive Radar frames. Then, a novel algorithm is adopted to estimate the vehicle's ego-pose. The final estimated pose will be aided by the INS and odometer. These steps are explained in detail in the following sub-sections.

Data Analysis and Preprocessing
The data analysis is performed in multiple steps (see Figure 1). a) Removing close and far points as they are considered ghost points. b) Removing points with a z value less than -2.0m as the Radar unit was fixed on the vehicle's roof, where z represents the vertical distance from the Radar unit. Any point cloud with z less than the vehicle's height means that this point is under the ground. c) Removing the moving objects by checking the speed of the detected objects. Thus, we can differentiate between static and moving objects and keep the constant objects or features. d) Removing outliers with intensity less than a threshold.

Data Association
After removing the noise, ghost points, and outliers, now it is the turn of data association to find the corresponding and matching features between Radar frames. The proposed method for data association is based on point-to-point matching. With the knowledge of Radar data rate (about 8 Hz) and the average vehicle speed (about 7 m/sec), the change in range and azimuth between any two points can be constrained as the range difference between any two corresponding points should be about 1m and the change in azimuth is about 1 o . Then, the corresponding points can be related.

The Proposed Algorithm for Ego-Motion Estimation
A novel algorithm was implemented to estimate the vehicle's ego-motion (see Figure 2). This algorithm calculates the coordinates of the objects ( , ) in the first Radar frame. It then uses these calculated coordinates and the range in the second frame to estimate the coordinates of the Radar center ( +∆ , +∆ ). The difference between the calculated Radar center in the second frame and the one in the first frame represents the transition between the two frames.
= cos ( ) The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2022 XXIV ISPRS Congress (2022 edition), 6-11 June 2022, Nice, France The rotation between the two frames can be estimated by calculating the difference between the bearing of objects at a time (t) and time ( + ∆ ).
, +∆ = − +∆ (4) If we have multiple corresponding objects between any two Radar frames, Least-squares is applied to obtain the final transformation and rotation between the two frames.
where = is the vector containing the corrections for the transition (dx and dy) and the rotation ( ) between any two Radar frames. A = the design matrix. = the measurement variance-covariance matrix. W = the misclosure vector.

Radar/odometer/INS Integration
This paper introduces a loosely coupled integration between Radar, INS, and odometer through a closed-loop EKF (see Figure 3). This paper will focus on the 2D navigation model, and the general 3D model is planned for future work. INS and odometer were used to estimate the Dead-Reckoning (DR) solution.
= azimuth angle measured clockwise from x-direction.
The estimated transition and rotation from Radar are integrated with the DR solution from IMU/odometer to smooth Radar solution, enhance the navigation solution, and limit IMU drift. The integration is implemented through a closed-loop EKF. The EKF is based on two models. The first model is the system model to describe the motion mode. The second model is the measurement model. is the error in the estimated azimuth angle; denotes the error in the estimated vertical gyroscope bias, which is modeled by a first-order Gauss-Markov process: Assuming the forward velocity is constant during ∆ , the dynamic matrix can be described by where is the inverse of the correlation time of the Gauss-Markov process.
The measurement model is described by: where: = the measurement vector H = the design matrix.
= the measurement noise.
The measurements are used to update the system states. The measurement vector in this work is the difference between the predicted position and azimuth from DR solution and the ones available from Radar solution: The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2022 XXIV ISPRS Congress (2022 edition), 6-11 June 2022, Nice, France The design matrix is defined by The above system and measurement models are employed in the EKF to obtain the final integrated Radar/DR solution. More details about the implementation of the EKF can be found in (Elsheikh & Noureldin, 2020;Liu et al., 2010)).

EXPERIMENTAL WORK AND RESULTS
The experimental work utilizes the Frequency Modulated Continuous Wave (FMCW) Radar UMRR-11 Type 132 in a short-range mode and Pixhawk 4 board containing a u-Blox GNSS and BMI055 Inertial Measurement Unit (IMU) sensors (see Figure 4). The Radar units and the board were mounted on the roof of a Honda Civic vehicle, as shown in Figure 5. Two real datasets were collected in two different suburban areas near the University of Calgary, Canada.  The experimental test was carried out with a maximum speed of 50 km/hr and an average speed of about 29 km/hr. The proposed Radar localization algorithm was applied and integrated with INS and odometer. A simulated GNSS signal outage was tested for different outage durations ranging from 60 seconds to 4 minutes. Figure 6 shows an example, from the first dataset, of the corresponding features between two successive Radar frames after applying the data association algorithm. With Radar/odometer/INS integration, the horizontal position RMSE for 2 minutes simulated GNSS outage was 8.99 m. The traveled distance was 655.18 m with a percentage error of 1.37%, as shown in Figure 7.    Figure 8.  On the other hand, if there are features in the surrounding environment and Radar can detect them, the vehicle's position accuracy from Radar/odometer/INS will be improved, e.g., the first dataset, especially for a long-time outage which will be beneficial to autonomous vehicles navigation.

CONCLUSION
In this paper, FMCW Radar was utilized to aid INS for autonomous navigation applications in GNSS denied environments. A novel algorithm was adopted for data association and ego-motion estimation. The proposed method was tested over two real datasets collected in suburban areas near the University of Calgary, Canada. Radar/odometer/INS integration was applied to smooth the Radar ego-motion solution, limit INS drift and compensate for GNSS signal outages. The integration of Radar, odometer, and INS has the advantage of working in different weather and light conditions. Moreover, this system can work indoors and outdoors which can meet the continuity requirements to work in the GNSS denied environments. Also, the proposed method can be applied in realtime. Thus, the proposed algorithm is useful for ego-motion estimation in autonomous navigation applications.

ACKNOWLEDGMENT
This research has been supported by the funding of Prof. Naser El-Sheimy from NSERC CREATE and Canada Research Chairs programs.