Unscented state estimation for rigid body motion on SE(3)

A state estimation scheme that uses a global representation of the configuration space for rigid body motion, is presented. This estimation scheme works as a deterministic filter which selects a set of sigma points obtained from the unscented transform based on exponential coordinates centered at the current state estimate. These sigma points and the state estimate are propagated in time along the dynamics until the next set of state measurements are obtained. This propagation (prediction) stage is carried out using a Lie group variational integrator, which discretizes the continuous equations of motion of the rigid body. The propagated sigma points and state estimate are enclosed by a minimum volume ellipsoid at the measurement instant. It is assumed that all states are measured at a constant measurement sample rate and that state measurement errors, expressed in the exponential coordinates, are bounded by an ellipsoidal bound. The update stage of the filter then consists of finding the minimum trace ellipsoid that contains the intersection of this measurement uncertainty bound and the minimum volume ellipsoid enclosing the propagated sigma points. This updated ellipsoid provides the filtered uncertainty bound and its center provides the updated state estimate. A new set of sigma points is selected from this ellipsoid and the propagation and update steps are repeated between measurement instants.