Estimating SE(3) elements using a dual quaternion based linear Kalman filter

Abstract

Abstract-Many applications in robotics such as registration, object tracking, sensor calibration, etc. use Kalman filters to estimate a time invariant SE(3) element by locally linearizing a non-linear measurement model. Linearization-based filters tend to suffer from inaccurate estimates, and in some cases divergence, in the presence of large initialization errors. In this work, we use a dual quaternion to represent the SE(3) element and use multiple measurements simultaneously to rewrite the measurement model in a truly linear form with state dependent measurement noise. Use of the linear measurement model bypasses the need for any linearization in prescribing the Kalman filter, resulting in accurate estimates while being less sensitive to initial estimation error. To show the broad applicability of this approach, we derive linear measurement models for applications that use either position measurements or pose measurements. A procedure to estimate the state dependent measurement uncertainty is also discussed. The efficacy of the formulation is illustrated using simulations and hardware experiments for two applications in robotics: rigid registration and sensor calibration.

Bingham Distribution-Based Linear Filter for Online Pose Estimation

“…The uncertainty in the state variables is often modeled using probability distributions functions (pdf) and the parameters of the pdf are updated after each measurement. In contrast to batch estimation methods, where there is no indication of when to stop collecting measurements, convergence of the state estimate and decrease in the state uncertainty provides clear indication of when to stop collecting measurements Gaussian Filtering Approaches: Several sequential estimation methods are based on Kalman filters, which model the states and measurements using Gaussian distributions [22,21,13,29]. Kalman filters by construction provide optimal state estimates when the process and measurement models are linear and the states and measurements are Gaussian distributed [15].…”

“…Linear Filtering Approach: Srivatsan et al [29] have recently developed a linear Kalman filter for pose estimation using dual quaternions and pairwise measurement update. While this method has been shown to be robust to errors in initial state estimate and sensor noise, it has a few drawbacks: (1) The uncertainty in the quaternions used for orientation estimate were modeled using Gaussians which do not consider the condition, q and − q represent the same rotation.…”

“…[29] used dual-quaternions and developed a linear Kalman filter which is robust to initial pose errors. In this work, a Bingham distribution is used to model the uncertainty in the pose parameters, and a linear measurement model is adapted from [29] to develop a linear filter for online pose estimation. The Bingham distribution is defined on a unit hypersphere and captures the bimodality of the unit-quaternions [3] (see Fig.…”

“…When compared to prior methods, the use of the Bingham distribution results in a more principled formulation that has lower computation time, because there is no normalization step or projection onto a hyper-sphere. Another advantage of our approach compared to existing methods [29,21,22] is the ability to update the pose using surface-normal measurements as well as simultaneous multiple measurements (as obtained from a stereo camera or lidar).…”

“…Inspired by [14,29], the pose is estimated by decoupling orientation from translation estimation, in this work. The method uses a Bingham distribution-based filtering (BF) for orientation estimation and a Gaussian distribution for translation estimation.…”