Radar and Visual Odometry Integrated System Aided Navigation for UAVS in GNSS Denied Environment

Drones are becoming increasingly significant for vast applications, such as firefighting, and rescue. While flying in challenging environments, reliable Global Navigation Satellite System (GNSS) measurements cannot be guaranteed all the time, and the Inertial Navigation System (INS) navigation solution will deteriorate dramatically. Although different aiding sensors, such as cameras, are proposed to reduce the effect of these drift errors, the positioning accuracy by using these techniques is still affected by some challenges, such as the lack of the observed features, inconsistent matches, illumination, and environmental conditions. This paper presents an integrated navigation system for Unmanned Aerial Vehicles (UAVs) in GNSS denied environments based on a Radar Odometry (RO) and an enhanced Visual Odometry (VO) to handle such challenges since the radar is immune against these issues. The estimated forward velocities of a vehicle from both the RO and the enhanced VO are fused with the Inertial Measurement Unit (IMU), barometer, and magnetometer measurements via an Extended Kalman Filter (EKF) to enhance the navigation accuracy during GNSS signal outages. The RO and VO are integrated into one integrated system to help overcome their limitations, since the RO measurements are affected while flying over non-flat terrain. Therefore, the integration of the VO is important in such scenarios. The experimental results demonstrate the proposed system’s ability to significantly enhance the 3D positioning accuracy during the GNSS signal outage.


Introduction
Over the past 10 years, Unmanned Aerial Vehicles (UAVs have become an essential tool for wide civil and military applications, such as first aid delivery, firefighting, rescue, downtown traffic monitoring, disaster management, border mentoring, and reconnaissance. The deployment of these UAVs in such aspects can help minimize human exposure to hazards and risks in dangerous circumstances. Most of the UAVs are commonly dependent on their onboard Global Navigation Satellite System (GNSS)/Inertial Navigation System (INS) integrated navigation systems to estimate where they are flying while performing their missions. The main challenge for this system configuration while flying in such cluttered environments is that the satellite signals might be disturbed in various scenarios, such as signals blockage, attenuation, multipath effects, jamming, spoofing in hostile areas, and flying in urban areas or natural canyons. In such harsh scenarios, the navigation solution is obtained only by the INS prior to the GNSS signal retrieval. Hence, the navigation solution will

Frequency Modulated Continuous Wave Radar Odometry
The proposed RO estimates the vehicle forward velocity and the height above the ground from its Range Doppler Map (RDM). This RO consists of data acquisition, target detection, velocity, and height above ground extraction.

Data Acquisition
This RO acquires the ranges and velocity measurements for the reflected ground scatterer based on an internal radar signal processing. During the flight, the attached micro radar is continuously transmitting a frequency modulated sawtooth chirps toward the ground objects : where is the sweep rate, is the frequency sweep time, and 0 is the initial transmitted frequency. This sweep rate can be obtained as follows:

Frequency Modulated Continuous Wave Radar Odometry
The proposed RO estimates the vehicle forward velocity and the height above the ground from its Range Doppler Map (RDM). This RO consists of data acquisition, target detection, velocity, and height above ground extraction.

Data Acquisition
This RO acquires the ranges and velocity measurements for the reflected ground scatterer based on an internal radar signal processing. During the flight, the attached micro radar is continuously transmitting a frequency modulated sawtooth chirps toward the ground objects f RF TX : where K f is the sweep rate, T is the frequency sweep time, and f 0 is the initial transmitted frequency. This sweep rate can be obtained as follows: where BW represents the transmitted chirp signal bandwidth. A roundtrip propagation time delay ∆t and a small frequency shift between the transmitted and received signals take place due to the range propagation effect. The time propagation delay for the reflected signals from each scatterer i is: where c is the speed of light, and r i is the range between the micro radar and each object which is located within the radar beamwidth. The originally transmitted signal is then mixed with the received signal in a low-pass filter to estimate the video signal x(t) that has a Beat frequency f b , which can be written as: The phase change of this video signal is then utilized to obtain the Doppler frequency f doppler . In each chirp, this video signal is sampled with a 264-ns sampling rate. A two-dimensional Fast Fourier Transform (FFT) is then applied on the sampled signal [28] to form the radar RDM for each of the three receiving antennae. A mean RDM is generated by averaging the RDM from all antennae. This mean RDM has 256 × 256 pixels with a 32-Bit amplitude value for each pixel, which is used to obtain the radar range and velocity measurements for different scatterers. Figure 2 illustrates an RDM image where the X-axis represents the velocity measurements and the Y-axis provides the range measurements. The 32-Bit amplitude value at each pixel represents the received echo signals strength from different ground scatterers.
where represents the transmitted chirp signal bandwidth. A roundtrip propagation time delay ∆ and a small frequency shift between the transmitted and received signals take place due to the range propagation effect. The time propagation delay for the reflected signals from each scatterer is: where is the speed of light, and is the range between the micro radar and each object which is located within the radar beamwidth. The originally transmitted signal is then mixed with the received signal in a low-pass filter to estimate the video signal ( ) that has a Beat frequency , which can be written as: The phase change of this video signal is then utilized to obtain the Doppler frequency . In each chirp, this video signal is sampled with a 264-ns sampling rate. A two-dimensional Fast Fourier Transform (FFT) is then applied on the sampled signal [28] to form the radar RDM for each of the three receiving antennae. A mean RDM is generated by averaging the RDM from all antennae. This mean RDM has 256 × 256 pixels with a 32-Bit amplitude value for each pixel, which is used to obtain the radar range and velocity measurements for different scatterers. Figure 2 illustrates an RDM image where the X-axis represents the velocity measurements and the Y-axis provides the range measurements. The 32-Bit amplitude value at each pixel represents the received echo signals strength from different ground scatterers. This RDM is acquired from the micro radar through an Ethernet cable after the radar signal processing inside the radar is performed. This generated map is then utilized for the target detection, velocity estimation, and height extraction purposes.

Target Detection and Data Extraction
The target detection process of radar is commonly achieved by utilizing the Constant False Alarm Rate (CFAR) algorithm. This algorithm adaptively estimates the threshold power level by locally comparing the power for the cell-under-test (CUT) against its neighborhood cells (background) This RDM is acquired from the micro radar through an Ethernet cable after the radar signal processing inside the radar is performed. This generated map is then utilized for the target detection, velocity estimation, and height extraction purposes.

. Target Detection and Data Extraction
The target detection process of radar is commonly achieved by utilizing the Constant False Alarm Rate (CFAR) algorithm. This algorithm adaptively estimates the threshold power level by locally comparing the power for the cell-under-test (CUT) against its neighborhood cells (background) [29]. The target detection is achieved when the power level of the CUT exceeds the estimated threshold level. Many ground radar stations and airborne radars are relying on the CFAR for the aircraft detection purpose. Although the CFAR is an effective detection approach for these applications, it is not effective for the proposed system since the ground scatterers have approximately the same power level on the RDM. The CFAR-detected targets for the RDM image of Figure 2 is shown in Figure 3 where CFAR could detect a part of the main reflected arc of the ground objects while it misses the rest of it. On the other hand, the CFAR can detect a false target (noise), which has a prominent power level from its background.  [29]. The target detection is achieved when the power level of the CUT exceeds the estimated threshold level. Many ground radar stations and airborne radars are relying on the CFAR for the aircraft detection purpose. Although the CFAR is an effective detection approach for these applications, it is not effective for the proposed system since the ground scatterers have approximately the same power level on the RDM. The CFAR-detected targets for the RDM image of Figure 2 is shown in Figure 3 where CFAR could detect a part of the main reflected arc of the ground objects while it misses the rest of it. On the other hand, the CFAR can detect a false target (noise), which has a prominent power level from its background. The first issue occurs when the CUT is picked inside a patched area of real ground scatterers. In this case, the CUT has approximately the same power level as its surrounding background and the CFAR could not detect all the targets since the CUT has the same power level as its neighborhood, which makes it not distinctive.
The second issue results from random noises, which have relatively high-power levels compared to its local neighborhood cells. Unlike the CFAR, which depends on the local neighborhood cells for target detection, an efficient target detection technique is proposed based on the Gaussian kernel and local maxima to avoid problems associated with the CFAR. The Gaussian kernel is convolved with the RDM as a filtering step to depress the isolated noise responses, which may affect the RO estimated height and velocity. A local maxima-based approach is then applied as a global threshold to obtain the strongest five candidates on the whole image which are considered to be the detected targets, as shown in Figure 4. The target detection process takes approximately 1.3 ms, which is convenient for real-time applications.
The radial velocity and the range for each detected target are then acquired directly from the Xand Y-axes of this image, respectively. These radial velocities are projected by the radar tilt angle, which is 60° with respect to the flight path direction in addition to the vehicle pitch angle. These projected components are then averaged to obtain the resultant forward velocity of the vehicle in the body frame. On the other hand, the ranges are projected and compensated by the same angles with respect to the vertical direction and then averaged to obtain the height above the ground level for the vehicle. The first issue occurs when the CUT is picked inside a patched area of real ground scatterers. In this case, the CUT has approximately the same power level as its surrounding background and the CFAR could not detect all the targets since the CUT has the same power level as its neighborhood, which makes it not distinctive.
The second issue results from random noises, which have relatively high-power levels compared to its local neighborhood cells. Unlike the CFAR, which depends on the local neighborhood cells for target detection, an efficient target detection technique is proposed based on the Gaussian kernel and local maxima to avoid problems associated with the CFAR. The Gaussian kernel is convolved with the RDM as a filtering step to depress the isolated noise responses, which may affect the RO estimated height and velocity. A local maxima-based approach is then applied as a global threshold to obtain the strongest five candidates on the whole image which are considered to be the detected targets, as shown in Figure 4. The target detection process takes approximately 1.3 ms, which is convenient for real-time applications.

Enhanced Moncular Visual Odometry
A GoPro HERO4 camera is attached to the quadcopter with a resolution of 1080 × 1920, and a video measurement rate of 30 frames per second. This camera is mounted on the UAV to have a downward facing orientation. The proposed VO consists of two main steps, which are the monocular VO based on the optical flow, and regression tree-based approach for the velocity drift error compensation.

Monocular Visual Odometry
The proposed VO extracts the optical flow vectors in the X and Y directions by detecting the features of interest from the video frames using the Speeded Up Robust Features (SURF) detector [30]. These detected features are then matched between successive frames. The main reason for utilizing the SURF algorithm is that it has a low computational load, which makes it more convenient for real-time operation. An M-estimator Sample Consensus (MSAC) algorithm is employed for outlier (incorrect match) rejection purpose. Suppose a point = [ , , ] in the space is projected by a pinhole camera model to an image plane at point = [ , , ], which is described as: where is the camera's focal length. Since the camera is mounted perpendicularly to a vehicle body, the coordinate is approximately equal to the distance between ground and camera's projection origin. The estimated ground distance using radar measurement is shown in Figure 5.
By having this ground distance, the displacement in the image plane (∆ , ∆ ) can be converted to a real-world displacement (∆ , ∆ ) as following: The displacement in the image plane is obtained after removing the outliers from the matched points between two successive frames. As the computed displacement (∆ , ∆ ) is usually in pixels, it is required to convert it into real-world units (e.g., meters), which is written as: The radial velocity and the range for each detected target are then acquired directly from the X-and Y-axes of this image, respectively. These radial velocities are projected by the radar tilt angle, which is 60 • with respect to the flight path direction in addition to the vehicle pitch angle. These projected components are then averaged to obtain the resultant forward velocity of the vehicle in the body frame. On the other hand, the ranges are projected and compensated by the same angles with respect to the vertical direction and then averaged to obtain the height above the ground level for the vehicle.

Enhanced Moncular Visual Odometry
A GoPro HERO4 camera is attached to the quadcopter with a resolution of 1080 × 1920, and a video measurement rate of 30 frames per second. This camera is mounted on the UAV to have a downward facing orientation. The proposed VO consists of two main steps, which are the monocular VO based on the optical flow, and regression tree-based approach for the velocity drift error compensation.

Monocular Visual Odometry
The proposed VO extracts the optical flow vectors in the X and Y directions by detecting the features of interest from the video frames using the Speeded Up Robust Features (SURF) detector [30]. These detected features are then matched between successive frames. The main reason for utilizing the SURF algorithm is that it has a low computational load, which makes it more convenient for real-time operation. An M-estimator Sample Consensus (MSAC) algorithm is employed for outlier (incorrect match) rejection purpose. Suppose a point P = [X, Y, Z] in the space is projected by a pinhole camera model to an image plane at point p = x pix , y pix , f cam , which is described as: where f cam is the camera's focal length. Since the camera is mounted perpendicularly to a vehicle body, the coordinate Z is approximately equal to the distance between ground and camera's projection origin. The estimated ground distance using radar measurement is shown in Figure 5.
the X and Y directions. These compensated vectors are then utilized to estimate the vehicle forward velocity V VO , which is described as: where ∆t is the time between two consecutive frames.

Velocity Compensation
During this velocity compensation phase, a regression tree-based approach is employed to enhance the accuracy of the estimated vehicle velocity from the monocular VO and compensate the employed approximations in the previous velocity estimation step. This training process is achieved during the availability of the GNSS signals. The GNSS signals are assumed to be available during the first 50 and 140 s of the first and second flights, respectively. The training process takes place during the first 40 s (from 1 to 40 s) for the first flight and the remaining 10 s (40 to 50 s) of the GNSS signals availability are utilized to obtain the weights for the predicted velocity from the regression trees. These weights are then utilized for the averaging process between the predicted velocity from the regression trees and the estimated vehicle forward velocity from the monocular VO. During the second flight, the whole collected data during the first flight and the first 125 s (from 1 to 125 s of the GNSS signal availability) of the second flight are utilized for the training purpose. The remaining 15 s (125 to 140 s of the GNSS signal availability) are utilized to obtain the weights for the predicted velocity from the regression trees.
The video frames are divided into 3 × 3 cells, and the optical flow vectors in the X and Y directions are then averaged inside each cell to offer a fixed number of vectors to the regression trees during this stage, as shown in Figure 6. Theses averaged optical flow vectors along each cell, the estimated forward velocity from the monocular VO, roll, pitch, and RO height are then utilized as inputs for the regression trees during the training session while the ground truth forward velocity, which is obtained from the GNSS/INS/magnetometer/barometer integration, is utilized as an output during this training phase. Figure 7 illustrates the proposed system architecture during the training session.
When the GNSS signals are unavailable, the trained regression trees start to estimate the vehicle forward velocity in an attempt to compensate the monocular VO drift errors caused by different reasons, such as featureless area, inconsistent matches, and the change of the camera calibration By having this ground distance, the displacement in the image plane (∆u, ∆v) can be converted to a real-world displacement (∆X, ∆Y) as following: The displacement in the image plane is obtained after removing the outliers from the matched points between two successive frames. As the computed displacement (∆u, ∆v) is usually in pixels, it is required to convert it into real-world units (e.g., meters), which is written as: where s is the pixel size. The optical vectors (u, v) are obtained by multiplying these image displacements by the camera's measurements rate. The gyro's measurements ω x , ω y are then utilized to compensate the vehicle rotational motion effect from the estimated optical flow vectors in the X and Y directions. These compensated vectors are then utilized to estimate the vehicle forward velocity V VO , which is described as: where ∆t is the time between two consecutive frames.

Velocity Compensation
During this velocity compensation phase, a regression tree-based approach is employed to enhance the accuracy of the estimated vehicle velocity from the monocular VO and compensate the employed approximations in the previous velocity estimation step. This training process is achieved during the availability of the GNSS signals. The GNSS signals are assumed to be available during the first 50 and 140 s of the first and second flights, respectively. The training process takes place during the first 40 s (from 1 to 40 s) for the first flight and the remaining 10 s (40 to 50 s) of the GNSS signals availability are utilized to obtain the weights for the predicted velocity from the regression trees. These weights are then utilized for the averaging process between the predicted velocity from the regression trees and the estimated vehicle forward velocity from the monocular VO. During the second flight, the whole collected data during the first flight and the first 125 s (from 1 to 125 s of the GNSS signal availability) of the second flight are utilized for the training purpose. The remaining The video frames are divided into 3 × 3 cells, and the optical flow vectors in the X and Y directions are then averaged inside each cell to offer a fixed number of vectors to the regression trees during this stage, as shown in Figure 6. Theses averaged optical flow vectors along each cell, the estimated forward velocity from the monocular VO, roll, pitch, and RO height are then utilized as inputs for the regression trees during the training session while the ground truth forward velocity, which is obtained from the GNSS/INS/magnetometer/barometer integration, is utilized as an output during this training phase. Figure 7 illustrates the proposed system architecture during the training session.
Sensors 2018, 18, 2776 9 of 27 parameters due to the strong vehicle vibration effect during the flight. A weighted average between the predicted velocity from the regression trees and the estimated velocity from the monocular VO is utilized as a measurements update for the EKF. These weights are obtained by computing the Root Mean Square (RMS) errors for the predicted velocity from the regression trees and the estimated velocity from the monocular VO with respect to the reference forward velocity from 40 to 50 s for the first flight and from 125 to 140 s for the second flight. The estimated forward velocity from the RO is also utilized as a measurement update during this GNSS outage period. Figure 8 illustrates the proposed system architecture during the prediction session. The regression tree-based approach is employed since it has the ability to handle the situation of missing inputs (optical flow vectors) in some image parts due to the lack of the observed features or inconsistent matches caused by repeated patterns.   parameters due to the strong vehicle vibration effect during the flight. A weighted average between the predicted velocity from the regression trees and the estimated velocity from the monocular VO is utilized as a measurements update for the EKF. These weights are obtained by computing the Root Mean Square (RMS) errors for the predicted velocity from the regression trees and the estimated velocity from the monocular VO with respect to the reference forward velocity from 40 to 50 s for the first flight and from 125 to 140 s for the second flight. The estimated forward velocity from the RO is also utilized as a measurement update during this GNSS outage period. Figure 8 illustrates the proposed system architecture during the prediction session. The regression tree-based approach is employed since it has the ability to handle the situation of missing inputs (optical flow vectors) in some image parts due to the lack of the observed features or inconsistent matches caused by repeated patterns.   When the GNSS signals are unavailable, the trained regression trees start to estimate the vehicle forward velocity in an attempt to compensate the monocular VO drift errors caused by different reasons, such as featureless area, inconsistent matches, and the change of the camera calibration parameters due to the strong vehicle vibration effect during the flight. A weighted average between the predicted velocity from the regression trees and the estimated velocity from the monocular VO is utilized as a measurements update for the EKF. These weights are obtained by computing the Root Mean Square (RMS) errors for the predicted velocity from the regression trees and the estimated velocity from the monocular VO with respect to the reference forward velocity from 40 to 50 s for the first flight and from 125 to 140 s for the second flight. The estimated forward velocity from the RO is also utilized as a measurement update during this GNSS outage period. Figure 8 illustrates the proposed system architecture during the prediction session. The regression tree-based approach is employed since it has the ability to handle the situation of missing inputs (optical flow vectors) in some image parts due to the lack of the observed features or inconsistent matches caused by repeated patterns.

Data Fusion
This section describes the data fusion between INS, magnetometer, barometer, enhanced VO, RO, and GNSS measurements during its availability in an EKF. The navigation states, which are the position, velocity, and attitudes in the navigation frame (n-frame) are derived from the IMU raw measurements through a mechanization process. The EKF error states vector include 21 states as follows: where r n , v n , n are the (position, velocity and attitude) error vector of INS mechanization, respectively, and b and d are the accelerometers bias and gyros drift, respectively. Finally, and are the accelerometers and gyros scale factor, respectively. The whole sensors are fused in loosely coupled fashion through two main steps inside the EKF, the prediction phase and the measurements update phase.

Prediction Model
The system model, which describes how different INS error states evolve with time, is obtained by linearly perturbing the mechanization equations and can be represented [31,32] as follows:

Data Fusion
This section describes the data fusion between INS, magnetometer, barometer, enhanced VO, RO, and GNSS measurements during its availability in an EKF. The navigation states, which are the position, velocity, and attitudes in the navigation frame (n-frame) are derived from the IMU raw measurements through a mechanization process. The EKF error states vector include 21 states as follows: where δr n , δv n , ε n are the (position, velocity and attitude) error vector of INS mechanization, respectively, and b and d are the accelerometers bias and gyros drift, respectively. Finally, s a and s g are the accelerometers and gyros scale factor, respectively. The whole sensors are fused in loosely coupled fashion through two main steps inside the EKF, the prediction phase and the measurements update phase.

Prediction Model
The system model, which describes how different INS error states evolve with time, is obtained by linearly perturbing the mechanization equations and can be represented [31,32] as follows: where Φ k,k+1 is the state transition matrix,x + k is the error states from the previous epoch,x − k+1 is the predicted error states, and w k is the system noise. The inertial sensor stochastic errors are modeled as a first-order Gauss-Markov process. The prediction of state-error covariance matrix P − k+1 at a certain epoch is: where Q k is the covariance matrix of the system noise, P + k is the error states covariance matrix from the previous epoch, and G k is the noise coefficient matrix.

Observation Model
The differences between the GNSS positioning measurements P where e P is the vector of GNSS position measurement errors and can be expressed as: where e n , e e , e d are the positioning errors in the North, East and Down, respectively.
where R is the equatorial earth radius, e is the eccentricity of the earth ellipsoid, and ϕ is the latitude. The positioning measurements which are utilized for updating the EKF are: (17) where the parameter l b GNSS × is the skew-symmetric form of the lever-arm between the GNSS antenna and the IMU in the body frame, C l b is the rotation matrix between the body and local level frame, δP ned IMU are the positions error states, and ∅ is the attitude errors which can be expressed in a skew-symmetric form E ned (or cross product ∅×) form [33] as: where n , e and d are the attitude errors in the North, East and Down directions, respectively. From Equation (17), the design matrix can be expressed as: Finally, the innovation sequence between the measurement updates z GNSS and the estimated measurementsẑ GNSS is obtained as: The heading measurements ψ mag is acquired from the magnetometer raw measurements (3D magnetic field components) as: where M x M y M z are the unit-vector measurements of magnetic north in the body frame, Φ is the roll angle, θ is the pitch angle and δ mag is the magnetic declination, which is the bearing the difference between the true north and magnetic north, and e ψ mag is the magnetometer heading measurement error. The estimated Direction Cosine Matrix (DCM)Ĉ l b , which is derived from the mechanization process during the EKF prediction stage, is utilized to compute the heading angleψ. The heading measurement, which is used for updating the EKF, is the difference between the estimated headingψ and the measured heading ψ mag and can be written as: The error equation can be obtained by perturbing Equation (22) as: Hence, the design matrix can be expressed as: The innovation sequence between the measurement update z mag and the estimated measurementŝ z mag is obtained as: The barometer is utilized to measure the height of the vehicle h baro as: where e h baro is the barometer height measurement error. The design matrix can be expressed as: The innovation sequence between the measurement update z baro and the estimated measurementŝ z baro is obtained as: The estimated velocity in the flight path direction, which is obtained from the RO, can be written as: where V s RO is the velocity acquired from the RO represented in the sensor frame and e V is the velocity measurement noise. The difference between the predicted forward velocity from the INS in the sensor frame and the extracted forward velocity from the RO is utilized as a measurement update during the GNSS signal outage. The attitude errors and the angular rate errors must be incorporated in the derived velocity in the mechanization process. The EKF measurement updates can be expressed as: whereV s RO is the computed RO forward velocity from the INS represented in the sensor frame, V ned IMU × is the skew-symmetric form of the velocity of the vehicle at the center of the IMU represented in the navigation frame, l b RO × is the skew-symmetric form of the lever arm between the RO and the IMU represented in the body frame, C b l is the rotation matrix between the navigation and body frame. δV ned IMU is the velocity error of the vehicle at the center of the IMU and δω b ib is the angular rate measurement error. From Equation (30), the design matrix can be expressed as: The innovation sequence between the measurement updates z V RO and the estimated measurementsẑ V RO is obtained as: The enhanced monocular VO velocity update is derived in the same manner as the RO velocity update. After the innovation sequence δz k and design matrix H k computation for each sensor, the Kalman gain K k , the updated statesx + k , and its updated covariance matrix P + k can be obtained as:

Experimental Results
The results shown in the upcoming subsections are obtained from two-real flights with 3DR Solo quadcopter equipped with a micro-FMCW radar and a GoPro Hero4 camera.

Hardware Setup
A GoPro Hero4 camera with a fisheye lens is attached to the UAV to get a High Definition (HD) video measurement with 30 frames per second. Two real flights are performed in different places.
During the first flight, the camera's field of view is adjusted to be wide with a resolution of 1080 × 1920, while the field of view is adjusted to be medium with the same resolution during the second flight. A K-MD2 radar module is attached to the quadcopter belly through a wooden frame. This radar has a 24-GHz transmitted frequency with one transmitter and three receiver antennae. The radar range is measured up to 300 m with a 1-m range resolution and velocity measurements up to 40 m/s with a 0.6-m/s resolution accuracy. The UAV has a compact size as the dimensions of the radar are 120 × 72 × 15 mm, the radar weight is 165 g which makes it suitable for small UAVs, and the power consumption of this radar is 7.2 watt at 12 V. Figure 9 illustrates the utilized radar.
The results shown in the upcoming subsections are obtained from two-real flights with 3DR Solo quadcopter equipped with a micro-FMCW radar and a GoPro Hero4 camera.

Hardware Setup
A GoPro Hero4 camera with a fisheye lens is attached to the UAV to get a High Definition (HD) video measurement with 30 frames per second. Two real flights are performed in different places. During the first flight, the camera's field of view is adjusted to be wide with a resolution of 1080 × 1920, while the field of view is adjusted to be medium with the same resolution during the second flight. A K-MD2 radar module is attached to the quadcopter belly through a wooden frame. This radar has a 24-GHz transmitted frequency with one transmitter and three receiver antennae. The radar range is measured up to 300 m with a 1-m range resolution and velocity measurements up to 40 m/s with a 0.6-m/s resolution accuracy. The UAV has a compact size as the dimensions of the radar are 120 × 72 × 15 mm, the radar weight is 165 g which makes it suitable for small UAVs, and the power consumption of this radar is 7.2 watt at 12 V. Figure 9 illustrates the utilized radar.

Figure 9. Utilized Frequency Modulated Continuous Wave (FMCW) radar (RF-beam).
This radar is attached to a UDOO X86 single board computer. This computer is based on Quad Core 64-bit new-generation X86 processors made by Intel ® . The attached camera and radar to the SOLO quadcopter are shown in Figure 10. A 3DR Solo Quadcopter is utilized during the flights 'missions. This UAV has a Pixhawk-2 autopilot with an InvenSense MPU-6000 MEMS IMU, an MS5611 barometer and a U-blox GPS. Figure 11 illustrates a block diagram of the hardware configuration.
The experiments were conducted in two different places, with different trajectories, and the radar was pitched by 60° from the quadcopter body. The first flight was performed over a football playground while the second flight was performed over multiple objects with different altitudes, such as a house, trees, grass, cars, and hangars. Figure 12 shows aerial images, which were obtained from the first and second flights. This radar is attached to a UDOO X86 single board computer. This computer is based on Quad Core 64-bit new-generation X86 processors made by Intel ® . The attached camera and radar to the SOLO quadcopter are shown in Figure 10. A 3DR Solo Quadcopter is utilized during the flights "missions". This UAV has a Pixhawk-2 autopilot with an InvenSense MPU-6000 MEMS IMU, an MS5611 barometer and a U-blox GPS. Figure 11 illustrates a block diagram of the hardware configuration.  The experiments were conducted in two different places, with different trajectories, and the radar was pitched by 60 • from the quadcopter body. The first flight was performed over a football playground while the second flight was performed over multiple objects with different altitudes, such as a house, trees, grass, cars, and hangars. Figure 12 shows aerial images, which were obtained from the first and second flights.

First Experiment
The first flight trajectory is composed of 10 waypoints during the total flight time of 393 s with a maximum speed of 5 m/s, as shown in Figure 13.  In Figure 14, the estimated velocity from the typical closed form monocular VO, the enhanced monocular VO, and the RO, with RMS error values of 1.29, 1.02, and 0.49 m/s, respectively, are compared to the UAV reference forward velocity in the body frame.
Three outage scenarios were carried, with different outage periods, ranging from 30 s to 113 s. The first outage period was performed for 30 s. Figures 15 and 16 show comparisons between the estimated 2D flight trajectory outage segments from the GNSS/INS integration (ground truth segment) and an enhanced monocular VO aided navigation system during the first flight for 30 and 113 s of GNSS signal outage, respectively. Figures 17 and 18 show comparisons between the estimated 2D flight trajectory outage segments from the GNSS/INS integration (ground truth segment) and the proposed integrated system aided navigation system during the first flight for 30 and 113 s of GNSS signal outage, respectively. Figure 19 demonstrates the ability of the proposed system (RO/enhanced monocular VO/magnetometer/barometer) to mitigate the INS drift errors when the GNSS signals get lost, and to enhance the 3D Root Mean Square Error (RMSE) positioning accuracy to be 3.2 m in 113 s. Figure 20 shows a comparison between the estimated 2D flight trajectory outage segments from the GNSS/INS integration (ground truth segment) and INS in a standalone mode during the first flight for 30 s of GNSS signal outage. Figure 21 illustrates the navigation errors for the INS in the standalone mode in the North and East directions during 30 s of GNSS signal outage. In Figure 14, the estimated velocity from the typical closed form monocular VO, the enhanced monocular VO, and the RO, with RMS error values of 1.29, 1.02, and 0.49 m/s, respectively, are compared to the UAV reference forward velocity in the body frame.  Three outage scenarios were carried, with different outage periods, ranging from 30 s to 113 s. The first outage period was performed for 30 s. Figures 15 and 16 show comparisons between the estimated 2D flight trajectory outage segments from the GNSS/INS integration (ground truth segment) and an enhanced monocular VO aided navigation system during the first flight for 30 and 113 s of GNSS signal outage, respectively. Figures 17 and 18 show comparisons between the estimated 2D flight trajectory outage segments from the GNSS/INS integration (ground truth segment) and the proposed integrated system aided navigation system during the first flight for 30 and 113 s of GNSS signal outage, respectively.                     Table 1 provides comparisons of the RMS errors for the position states, which are obtained from the INS in a standalone mode, and enhanced monocular VO, and the proposed integrated system aided navigation during the GNSS outage periods. The results demonstrate the ability of the proposed integrated system to reduce the 3D positioning errors to 2.06% for 30 s, and 0.13% during 113 s of the INS drift errors in the standalone mode during the GNSS signals outages period.
The results also demonstrate the proposed integrated system's capability of reducing the 3D positioning RMS errors to 78.04%, 71.65%, and 65.71% of the enhanced monocular VO. Figure 22 shows a comparison between the RMS errors of the 3D positioning for the proposed integrated system aided navigation during three GNSS outage periods ranged from 30 s to 113 s.   Table 1 provides comparisons of the RMS errors for the position states, which are obtained from the INS in a standalone mode, and enhanced monocular VO, and the proposed integrated system aided navigation during the GNSS outage periods. The results demonstrate the ability of the proposed integrated system to reduce the 3D positioning errors to 2.06% for 30 s, and 0.13% during 113 s of the INS drift errors in the standalone mode during the GNSS signals outages period. The results also demonstrate the proposed integrated system's capability of reducing the 3D positioning RMS errors to 78.04%, 71.65%, and 65.71% of the enhanced monocular VO. Figure 22 shows a comparison between the RMS errors of the 3D positioning for the proposed integrated system aided navigation during three GNSS outage periods ranged from 30 s to 113 s.

Second Experiment
The second flight trajectory is composed of 18 waypoints of total flight time 393 s, with a maximum speed of 5 m/s, as shown in Figure 23. In Figure 24, the estimated velocity from the typical closed form monocular VO, the enhanced monocular VO, and the RO, with RMS error values of 0.61, 0.53, and 0.75 m/s, respectively, are compared to the UAV reference forward velocity in the body frame.
The accuracy for the estimated velocity from the RO was slightly less than those of the monocular VO, and the enhanced monocular VO, because the flight was performed over multiple objects with different altitudes, ranges, and angles.

Second Experiment
The second flight trajectory is composed of 18 waypoints of total flight time 393 s, with a maximum speed of 5 m/s, as shown in Figure 23.

Second Experiment
The second flight trajectory is composed of 18 waypoints of total flight time 393 s, with a maximum speed of 5 m/s, as shown in Figure 23. In Figure 24, the estimated velocity from the typical closed form monocular VO, the enhanced monocular VO, and the RO, with RMS error values of 0.61, 0.53, and 0.75 m/s, respectively, are compared to the UAV reference forward velocity in the body frame.
The accuracy for the estimated velocity from the RO was slightly less than those of the monocular VO, and the enhanced monocular VO, because the flight was performed over multiple objects with different altitudes, ranges, and angles. In Figure 24, the estimated velocity from the typical closed form monocular VO, the enhanced monocular VO, and the RO, with RMS error values of 0.61, 0.53, and 0.75 m/s, respectively, are compared to the UAV reference forward velocity in the body frame.   The accuracy for the estimated velocity from the RO was slightly less than those of the monocular VO, and the enhanced monocular VO, because the flight was performed over multiple objects with different altitudes, ranges, and angles.
Four outage scenarios were carried, with different outage periods, ranging from 60 s to 240 s. The first outage period was performed for 60 s. Figures 25 and 26 show comparisons between the estimated 2D flight trajectory outage segments from the GNSS/INS integration (ground truth segment) and enhanced monocular VO aided navigation during the second flight for 60 s and 240 s of GNSS signal outage, respectively.                       Table 2 provides comparisons of the RMS errors for the position states, which are obtained from the INS in a standalone mode, and the enhanced monocular VO, and the proposed integrated system aided navigation during the GNSS outages periods. The results demonstrate the ability of the proposed integrated system to reduce the 3D positioning errors to 3.09 % for 60 s, and 0.1% during 240 s of the INS drift errors in a standalone mode during the period of GNSS signal outages.     Table 2 provides comparisons of the RMS errors for the position states, which are obtained from the INS in a standalone mode, and the enhanced monocular VO, and the proposed integrated system aided navigation during the GNSS outages periods. The results demonstrate the ability of the proposed integrated system to reduce the 3D positioning errors to 3.09 % for 60 s, and 0.1% during 240 s of the INS drift errors in a standalone mode during the period of GNSS signal outages.   Table 2 provides comparisons of the RMS errors for the position states, which are obtained from the INS in a standalone mode, and the enhanced monocular VO, and the proposed integrated system aided navigation during the GNSS outages periods. The results demonstrate the ability of the proposed integrated system to reduce the 3D positioning errors to 3.09 % for 60 s, and 0.1% during 240 s of the INS drift errors in a standalone mode during the period of GNSS signal outages. The results demonstrate the proposed integrated system aided navigation is capable of reducing the positioning RMSEs to 92.53%, 99.41%, and 92.38% of the enhanced monocular VO during 60, 120, and 180 s of GNSS signal outages, respectively, while it has almost the same accuracy as the enhanced monocular VO during 240 s. Larger data than that for the first flight is used to train the enhanced monocular VO, which helps with an accurate estimate for the vehicle forward velocity. Figure 32 shows a comparison of the 3D positioning RMSEs of the proposed integrated system aided navigation during four GNSS outage periods ranged from 60 s to 240 s. The results demonstrate the proposed integrated system aided navigation is capable of reducing the positioning RMSEs to 92.53%, 99.41%, and 92.38% of the enhanced monocular VO during 60, 120, and 180 s of GNSS signal outages, respectively, while it has almost the same accuracy as the enhanced monocular VO during 240 s. Larger data than that for the first flight is used to train the enhanced monocular VO, which helps with an accurate estimate for the vehicle forward velocity. Figure 32 shows a comparison of the 3D positioning RMSEs of the proposed integrated system aided navigation during four GNSS outage periods ranged from 60 s to 240 s.

Conclusions
An integrated navigation system for UAVs in GNSS denied environments is proposed based on micro FMCW radar and a single camera. The vehicle's forward velocity is estimated from both the RO and the enhanced VO to enhance the INS navigation accuracy during GNSS signal outages. In addition, the estimated height from the RO is utilized to resolve the monocular VO's scale ambiguity. An efficient target detection approach is proposed to detect the ground objects based on a Gaussian kernel and local maxima algorithms. These detected targets are then utilized to estimate the forward velocity and the height above ground level for the vehicle. An optical flow and regression tree-based approaches are utilized to implement the enhanced monocular VO. The optical flow is employed for

Conclusions
An integrated navigation system for UAVs in GNSS denied environments is proposed based on micro FMCW radar and a single camera. The vehicle's forward velocity is estimated from both the RO and the enhanced VO to enhance the INS navigation accuracy during GNSS signal outages.
In addition, the estimated height from the RO is utilized to resolve the monocular VO's scale ambiguity. An efficient target detection approach is proposed to detect the ground objects based on a Gaussian kernel and local maxima algorithms. These detected targets are then utilized to estimate the forward velocity and the height above ground level for the vehicle. An optical flow and regression tree-based approaches are utilized to implement the enhanced monocular VO. The optical flow is employed for the forward velocity estimation purpose, while its associated drift errors are compensated based on a trained regression tree model. Theses estimated forward velocities from the RO and enhanced VO are then fused with the IMU, barometer, and magnetometer measurements via an EKF. The experimental results demonstrate the proposed system's ability to enhance the average 3D positioning errors for the first flight by 98.65%, and to by 99.04% for the second flight with respect to the INS on the stand alone mode. In addition, it improves the average 3D positioning accuracy by 28.21% for the first flight, and 4.72% for the second flight with respect to the enhanced monocular VO.
Unlike other proposed RO methods which utilize a large unmanned aircraft or large radar or even simulate the flight missions, the proposed system utilizes a small SOLO quadcopter and a lightweight micro radar during a real flight. Such small quadcopters are typically utilized in many missions, such as search, rescue, and disaster management. The proposed algorithm has been evaluated in a generic and typical maneuvering scenario. It also avoids the various assumptions imposed by many other researches, such as straight flights, constant velocities, leveled flights, and flying over flat terrain. The RO and VO are integrated into one integrated system to help overcome their limitations. The proposed RO provides a more accurate forward velocity estimation than the enhanced monocular VO while flying over a flat terrain and it is slightly worse than the enhanced monocular VO while flying over non-flat terrain. On the other hand, the radar is immune against the environmental changes and can operate in featureless areas.