Radar Odometry on $SE(3)$ With Constant Velocity Motion Prior

Abstract

This letter presents an approach to estimating vehicle odometry on SE(3) using a high resolution automotive radar sensor. We combine a constant velocity SE(3) motion prior with a 3D radar point cloud measurement model in a sliding window optimization scheme. We leverage radar's highly precise radial velocity measurement to compensate for point cloud sparsity and improve data association. Our approach is tested using real-world measurements from a prototype high-resolution radar sensor. We demonstrate the results of this approach for 6D motion estimation. In addition, we show that our approach is comparable to state-of-the-art SE(2) radar odometry estimates while running a full order of magnitude faster.