An EKF for Lie Groups with Application to Crane Load Dynamics

Abstract

An extended Kalman filter (EKF) for systems with configuration given by matrix Lie groups is presented. The error dynamics are given by the logarithm of the Lie group and are based on the kinematic differential equation of the logarithm, which is given in terms of the Jacobian of the Lie group. The probability distribution is also described in terms of the logarithm as a concentrated Gaussian distribution that is a tightly focused distribution around the identity of the Lie group. The filter is applied to estimation on SO(3) a case where a stereo camera setup tracks a crane wire with a payload. The wire, which is under tension and forms a line is monitored by two 2D-cameras, and a line detector is used to obtain a description of how the wire is projected onto each image plane. A model of a spherical pendulum is applied and the estimator is validated by applying it on simulated data, as well as experimental data.

The Integrated Probabilistic Data Association Filter Adapted to Lie Groups

“…This form is similar to the one found in [32] and is an exact approximation of the continuous system model provided that the Lie group is commutative. If the Lie group is not commutative, it is a close approximation.…”

Unscented Kalman Filter and Control in TSE(3) with Applications to Spacecraft Dynamics

“…This is due to the fact that SE(3) is a nonlinear manifold and not a vectorial space [35]. In this paper, the formulation covered in [26,30] is used to accommodate a stochastic process in the model. Since the source of noise is assumed to be in vector space, the exponential map exp(•) is used to map it into SE(3) as…”

“…According Fig. 2, the EKF on SE(3) [26] is able to achieve an higher accuracy of DEKF on SE(3) [28] and lower accuracy than the UKF on SE(3) [30] and the proposed UKF on TSE(3). The latter one has proved to be particularly robust even with noisy measurements, inaccurate initial conditions, and low sampling frequency.…”

“…State filter design on SE(3), with application to robotics, has been of growing interest in the literature. Extended Kalman filter (EKF) on SE(3) [26,27], discrete-EKF (D-EKF) on SE(3) [28,29], and unscented Kalman filter (UKF) on SE(3) [30][31][32] are some examples. According to the literature, these estimators are more accurate than their counterparts designed in Euclidean space, although formulation development of the filter design on SE(3) is more complex than that in Euclidean space.…”

“…The navigation filter is designed through a direct UKF based on the unscented transform (UT) described in [33], and the mathematical formalism in [34] is revised to describe the dynamic system characterized by stochastic processes in a compact form on TSE(3). The stochasticity is treated on SE(3) and its tangent bundle TSE(3) as discussed in [26,30,35] and a retraction function between the manifold and the Euclidean space [36,37], the inverse of which is used to encode the UT sigma points onto the manifold and decode them from the manifold, respectively. The regulation controller in [17] is extended to design an almost globally asymptotically stable Morse-Lyapunov based tracking control algorithm.…”