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.