Geometric Savitzky-Golay Filtering of Noisy Rotations on SO(3) with Simultaneous Angular Velocity and Acceleration Estimation

This paper focuses on the problem of smoothing a rotation trajectory corrupted by noise, while simultaneously estimating its corresponding angular velocity and angular acceleration. To this end, we develop a geometric version of the Savitzky-Golay filter on SO(3) that avoids following the conventional practice of first converting the rotation trajectory into Euler-like angles, performing the filtering in this new set of local coordinates, and finally converting the result back on SO (3). In particular, the estimation of the angular acceleration requires the computation of the right-trivialized second covariant derivative of the exponential map on SO (3) with respect to the (+) Cartan-Schouten connection. We provide an explicit expression for this derivative, creating a link to seemingly unrelated existing results concerning the first derivative of the exponential map on SE (3). A numerical example is provided in which we demonstrate the effectiveness and straightforward applicability of the proposed approach. An open implementation of the new geometric Savitzky-Golay filter is also provided.