Invariant Trapezoidal Kalman Filter for Application to Attitude Estimation

This paper incorporates formal concepts of invariance of numerical integration schemes into the design of Kalman filters. In general terms, invariant discretizations of dynamical systems can form the basis for the derivation of the covariance propagation during the prediction and update phases of a Kalman filter, and this paper presents a framework to that effect. Specifically, natural invariants of angular motion are introduced, as part of a symmetry-preserving trapezoidal integration rule, to form the basis for the derivation of a discrete-time invariant Kalman filter for an attitude estimation problem. The proposed filter is realized by expressing all zero-mean random variables in the Lie algebra, while the state manipulations are performed in special orthogonal group 3. Simulation and experimental results are obtained using a neutrally buoyant spherical blimp to validate the proposed method against state-of-the-art Kalman filters in a broad range of angular speeds and sampling rates. Furthermore, the results support claims of invariance of the estimator with respect to angular speed, as well as the preservation of the system's symmetries through the covariance calculations.