The SO(3) and SE(3) Lie Algebras of Rigid Body Rotations and Motions and their Application to Discrete Integration, Gradient Descent Optimization, and State Estimation

Common mathematical techniques such as discrete integration, gradient descent optimization, and state estimation (exemplified by the Runge-Kutta method, Gauss-Newton minimization, and extended Kalman filter or EKF, respectively), rely on linear algebra and hence are only applicable to state vectors belonging to Euclidean spaces when implemented as described in the literature. This article describes how to modify these methods so they can be applied to non Euclidean state vectors, such as those containing rotations and full motions of rigid bodies. To do so, this article provides an in-depth review of the SO(3) and SE(3) Lie groups, known as the special orthogonal and special Euclidean groups of R3, which represent the rigid body rotations and motions, placing special emphasis on the different possible representations, their tangent spaces, the analysis of perturbations, and in particular the definitions of the jacobians required to employ the previously mentioned calculus methods.