Unscented Kalman filtering on Lie groups

Abstract

In this paper, we first consider a simple Bayesian fusion problem in a matrix Lie group, and propose to tackle it using the unscented transform. The method is then leveraged to derive two simple alternative unscented Kalman filters on Lie groups, for both cases of noisy partial measurements of the state, and full state noisy measurements of the state on the group. The general method is applied to a robot localization problem, and results based on experimental data combined with extensive Monte-Carlo simulations at various noise levels illustrate the superiority of the approach over the standard UKF.

Visual-Inertial Navigation: A Concise Review

“…We have introduced the optimal state constraint (OSC)-EKF [54,55] that first optimally extracts all the information contained in the visual measurements about the relative camera poses in a sliding window and then uses these inferred relative-pose measurements in the EKF update. The (right) invariant Kalman filter [56] was recently employed to improve filter consistency [25,57,58,59,60], as well as the (iterated) EKF that was also used for VINS in robocentric formulations [22,61,62,63]. On the other hand, in the EKF framework, different geometric features besides points have also been exploited to improve VINS performance, for example, line features used in [64,65,66,67,68] and plane features in [69,70,71,72].…”

A New Approach to 3D ICP Covariance Estimation

“…Computation ofQ wrong conv is of greater importance as in practice it largely dominatesQ at conv . We propose to compute it in a deterministic derivative-free method, in which we adapt the unscented transform [21] for the pose T ∈ SE(3) by following [22,26]. The advantages of using our unscented based method rather than Monte-Carlo sampling are fourfold: 1) it is deterministic; 2) it keeps computationally reasonable by adding only 12 ICP registrations which are easily parallelisable; 3) it explicitly computes the cross-covariance matrix betweenT icp andT odo as a by-product without extra computational operations; and 4) it scales with Q odo , i.e.…”

“…We computeQ wrong conv and infer the cross-covariancê Q cross = JQ odo between propagated and prior distributions as a by-product in respectively steps 4) and 6). We derive the algorithm by following [22] for pose measurement, zero-mean prior distribution, and where we set α = 1. Note that estimateξ icp computed in step 5) allows correcting or rejecting ICP registration failure, albeit beyond the scope of the present paper concerned with ICP estimate uncertainty assessment.…”

“…The definition of the uncertainty in the Lie algebra of SE(3) in (3) explains the shapes of the "ellipses", see e.g. [22,26]. The proposed method obtains more accurate (proximity to ground truth) and consistent (discrepancy w.r.t.…”

Invariant smoothing on Lie Groups

“…Note that a continuous-time counterpart of these dynamics also exists in the theory of control on Lie groups [2]. Theorem 1 (from [7]): f defines a group-affine dynamics if and only if g L (χ) =: f (Id) −1 f (χ) is a group automorphism, i.e., satisfies ∀a, b, g L (ab) = g L (a)g L (b), and g L (a −1 ) = g L (a) −1 (15) In that case, the Lie-group/Lie algebra morphism correspondance ensures the existence of a q × q matrix G L such that ∀ξ , g L (exp(ξ )) = exp(G L ξ ).…”