RAVE: A Framework for Radar Ego-Velocity Estimation

The proposed RAVE framework is suitable for radar sensors that provide 3D coordinates ($x$,$y$,$z$) and Doppler shift measurements of the target, i.e., relative velocity values. The ego velocity is estimated using a single scan, a method commonly referred to as an instantaneous method. Upon arrival of the radar data, we first perform zero-velocity detection similar to [6]. If the median of the Doppler velocity measurements in the point cloud falls below an user-defined minimum threshold and a relatively small percentage of the data (e.g., $25$%) does not meet this criterion, we conclude that the velocity is zero.

Ego-velocity estimation typically assumes that the majority of radar detections $\mathrm{D}_{i,k}$ ($i\in(1,M)$) within a single scan at a time instant $k$ are static and provide Doppler velocity measurements $\mathrm{v}_{\mathrm{D},k}$ and 3D object positions ${}_{\mathrm{R}}\mathbf{p}_{\mathrm{RD},\mathrm{i},k}$. Here an expression ${}_{\mathrm{{{{\scalebox{0.56}{A}}}}}}\mathbf{{t}_{{{\scalebox{0.56}{BC}}}}}$ represents a vector from frame $\mathcal{F}_{{\scalebox{0.56}{B}}}$ to frame $\mathcal{F}_{{\scalebox{0.56}{C}}}$ expressed in frame $\mathcal{F}_{{\scalebox{0.56}{A}}}$. Any valid static radar detection measurement must satisfy the following conditions:

where $\mathrm{{}_{{R}}}\mathrm{\mathbf{v}}_{{{R},k}}$ is the 3D translational radar velocity at the time instant $k$. Hence, with at least three non-coplanar raw radar measurements, it is possible to compute the 3D translational radar velocity [22]. In practice, radar point clouds often contain outliers in the form of non-stationary objects, ghost targets or alike. Similarly to the state-of-the-art methods, we distinguish the process of outlier rejection from the velocity estimation (using inliers), which constitutes a separate, modular part of the proposed framework.

Once inliers are determined, we construct a custom loss function, offering the flexibility to use different robust loss functions such as Cauchy, Huber, and General norm robust loss function presented in [23], from which all other loss functions (e.g., German-McClure, L1-L2, L2, Welsch/Leclerc) can be derived. The simplest form of a general loss function is defined as follows:

where $\alpha\in{\rm I\!R}$ represents shape parameters controlling robustness, and $c>0$ is a scale parameter regulating the size of the quadratic bowl near $x=0$ [23]. The introduction of a robust loss function can significantly reduce the effects of outliers that lead to errors in an estimate. After constructing the loss function, we solve the optimization problem using the Broyden–Fletcher–Goldfarb–Shanno (BFGS) algorithm [24]. The obtained velocity estimates are then filtered using the proposed filter, which is described in the following section. The proposed framework is illustrated in Fig. 2.

Since the velocity estimator at times produces unlikely velocity estimates, we implement a simplified filter to check the estimated velocity feasibility. The reason for this is that we have encountered situations where outlier rejection methods had difficulties in correctly identifying outliers, especially in scenarios with numerous moving objects, leading to unlikely velocity estimates.

For each new estimate, we apply the sliding window technique and calculate the average velocity norm based on the last $N$ estimates. The estimate is not kept if the difference between the mean and the newly estimated velocity norm is larger than a pre-specified threshold $T_{1}$, while the difference between the last and the current estimate is also too large, implying an acceleration above a threshold $a_{max}$. It is crucial to choose an appropriate value for $N$ to ensure that it represents relatively recent values, while the thresholds $T_{1}$ and $a_{max}$ should not be set too low, as this could lead to the rejection of accurately estimated values during fast movements. Using a larger window size requires a higher $T_{1}$ value as the moving average would be “slower”. It is also important to adjust the $N$ and $T_{1}$ parameters according to the sampling frequency of the radar, as a lower sampling frequency for the same $N$ requires a higher $T_{1}$ value than a higher sampling frequency. This approach is intended to filter out highly unlikely estimates, which are rare but can significantly degrade the accuracy in downstream tasks, such as odometry and SLAM.

When filling up an empty queue, we first check only the difference between the last and the current estimate. In our implementation, we do not consider the possibility of incorrect estimates at the first measurement, since most sequences start in a steady state where zero velocity detection provides reliable estimates. Finally, the correctly estimated velocity values that have successfully passed our filter can serve as initial values for the subsequent steps. This is especially important when using the robust Cauchy kernel loss, which is sensitive to initial values. The proposed filtering algorithm is summarized in Algorithm 1.

Due to the modular nature of the proposed framework, there are several parameters that should be chosen depending on the context of the problem at hand. In our implementation, we have explicitly specified some of these parameters for which we have empirically found that they lead to accurate velocity estimation for commonly used radars. We set the threshold for zero velocity detection to 0.05 m/s. For the proposed sliding-window filter, we chose the size of the filter queue $N=5$, the norm difference threshold $T_{1}=7.5$ m/s, and the maximum translational acceleration $a_{max}=10$ m/s².

For outlier rejection, most existing radar ego-velocity estimation methods use a 3-point RANSAC approach [6, 7]. RAVE also implements RANSAC, but it additionally implements GNC and MLESAC outlier rejection methods. Furthermore, our implementation supports multiple loss functions. RAVE allows outlier rejection and velocity estimation to be combined into a unified step by using the robust Cauchy loss function for the velocity estimation loss, similar to [16]. If the inliers are identified using RANSAC or MLESAC, the velocity estimation loss function can be generated using least squares (LS), truncated LS (TLS), weighted LS (WLS) with the signal-to-noise ration used for weighting, weighted truncated LS (WTLS), or any other custom optimization loss functions tailored to a particular problem. The choice of outlier rejection method and optimization loss function and their advantages and disadvantages are analyzed and discussed in the following section.

We evaluate the performance of RAVE on three open-source datasets: the IRS dataset, the ColoRadar dataset, and the VoD dataset.

The IRS dataset is the Radar Thermal Visual Inertial Dataset, which includes a wide range of indoor and outdoor sequences. It consists of a powerful IMU (Analog Devices ADIS16448), an FCMW radar (Texas Instruments IWR6843AOP) and a monocular grayscale camera (IDS UI-3241). The sensors are synchronized using hardware triggers and synchronization signals via a microcontroller board. The ground truth odometry data for some sequences are provided by the Vicon motion capture system (MoCap), while the ground truth data for the remaining sequences are analyzed using VINS [25] with loop closures.

ColoRadar, the 3D millimeter wave radar dataset, contains more than two hours of data collected in a large indoor and outdoor environment using a handheld sensor rig for robotic mapping and state estimation. This rig is equipped with two FMCW radar sensors (TI MMWACSRF-EVM, TI AWR1843BOOST-EVM), a 3D lidar (Ouster OS1) and an IMU (Lord Microstrain 3DM-GX5-25). The high-precision ground truth is achieved either with Vicon poses or with a globally optimized pose graph with IMU, lidar and loop closure constraints using [26].

The View-of-Delft (VoD) dataset [21] is a vehicle dataset containing 8693 frames of synchronized and calibrated 64-layer LiDAR (Velodyne HDL-64 S3), a (stereo) camera pair (1936×1216 px) and 3+1D radar data (ZF FR-Gen21 3+1D) acquired in complex urban traffic, as well as odometry data estimated using RTK-GPS, IMU and wheel odometry. The dataset is synchronized so that the LiDAR serves as the primary sensor, with the timestamps of the nearest camera and radar data rewritten to match the LiDAR timestamps. This synchronization method poses a challenge in distinguishing poses for velocity estimation. Therefore, we decided to perform a comparison with the MOVRO [4] odometry framework using different radar ego-velocity estimation methods.

For the IRS dataset, our evaluation is specifically focused on Mocap sequences, as they contain the ground truth velocity parameters derived from the Vicon motion capture system, from which we obtain the ground truth translation velocity parameters in the radar’s frame using the Euler’s equation. Euler’s equation for rigid bodies calculates velocity $\mathbf{v}_{\mathrm{a}}$ from $\mathbf{v}_{\mathrm{b}}$ using the following equation:

where $\boldsymbol{\omega}_{\mathrm{b}}$ and $\mathbf{p}_{\mathrm{ba}}$ denote the angular velocity of a rigid body around point b and the position vector from point b to point a, respectively. Due to negligible translational distance between the ground truth origin and the radar frame within the IRS dataset, we opted to neglect $\boldsymbol{\omega}\times\mathbf{p}_{\mathrm{ab}}$ part which simplifies ground truth velocity calculation.

For ColoRadar sequences, ground truth calculation we followed the same procedure as in [27]. To obtain ground truth ego-velocity data in time of estimates, we use linear interpolation.

To evaluate the effect of the filter described in Algorithm 1, we estimate the ego-velocity of the radar on three IRS sequences with Mocap ground truth. We use a common approach with RANSAC for outlier rejections and LS as loss function, both with and without the proposed filter. The obtained results are shown in Table I. Outliers have a larger impact on the root mean square error (RMSE) than on the average velocity error (AVE). Overall, our filter reduced both errors, especially in difficult scenarios (e.g. Mocap difficult), while similar results were obtained in other cases. The Mocap easy sequence does not contain any sudden motion changes, which explains the same error value with and without the filter. A visualization of the velocity estimation for the Mocap dark fast sequence, shown in Fig. (3), illustrates the positive effect of the filter. In this particular case, our filter identified and discarded 3 outliers in the ego-velocity estimates out of a total of 789 ego-velocity estimates. Due to the performance improvement of the proposed filter, it will be used for the remaining experiments in RAVE to mitigate the impact of outliers.

Next, we performed a comprehensive analysis of outlier rejection and loss function methods on IRS dataset sequences with Mocap ground truth (Table (II)) and on three ColoRadar sequences using a low-cost single-chip radar (Table (III)). We compared RANSAC and MLESAC as outlier rejection techniques, using LS, TLS, and robust Cauchy norm to construct the optimization loss function. We also evaluated the performance of GNC, Cauchy, and Huber robust norms without outlier rejection.

As expected, the accuracy of ego-velocity estimation depends on the difficulty of the sequence, i.e., the Mocap difficult and Mocap dark fast sequences have larger errors than the Mocap easy sequence of the IRS dataset. From the results obtained, it can be concluded that the robust Cauchy kernel loss provides the best performance in velocity estimation. For the sequences of the IRS dataset, it is more accurate without RANSAC or MLESAC, while for the sequences of the Coloradar dataset, the combination with RANSAC provides the most accurate results, albeit with a longer runtime of the algorithm compared to the other methods. It is important to note that these differences can be attributed to the larger point cloud in the Coloradar dataset, which contains higher number of outliers. Furthermore, the TLS loss does not show a significant advantage over the simple LS. A similar conclusion was drawn in [6], where the authors found that orthogonal distance refinement does not provide significant improvements over LS. The GNC method does not provide competitive results compared to RANSAC in combination with LS, possibly due to high noise in the low-cost radar data. Fig. (4) shows comparison of the velocity estimation for three different methods (RANSAC+LS, RANSAC+TLS, Cauchy) with the smallest RMSE on the aspen_run0 sequence. The lowest ego-velocity accuracy is observed in the z-axis, which is due to the lower resolution of the radar at elevation angles compared to azimuth angles.

We evaluate the RAVE velocity estimation within the MOVRO [4] odometry framework on the VoD dataset. The relative pose error (RPE), i.e., the relative translation error $t_{rel}$ and rotation error $r_{rel}$, is used to measure the performance. MOVRO integrates radar ego velocity measurements with monocular odometry pose data. Monocular odometry poses are analyzed using ov2slam  [28], while four different methods were used for the radar velocity measurements: the reve package  [6] and our implementation of RANSAC+LS, RANSAC+Cauchy, and the Cauchy method. The results obtained for three VoD sequences are shown in Table. IV, while the visualization of the trajectories can be seen in Fig. 5. We can see the benefit of improved ego-velocity accuracy and discarded estimation outliers on odometry accuracy, especially on $t_{rel}$, where the proposed ego-velocity estimation method outperforms a state-of-the-art ego-velocity estimation method with each tested combination of outlier rejection and optimization loss functions [6].