Sensor Fusion for Underwater Vehicle Navigation Compensating Misalignment Using Lie Theory

Abstract

This paper presents a sensor fusion method for navigation of unmanned underwater vehicles. The method combines Lie theory into Kalman filter to estimate and compensate for the misalignment between the sensors: inertial navigation system and Doppler Velocity Log (DVL). In the process and measurement model equations, a 3-dimensional Euclidean group (SE(3)) and 3-sphere space (S3) are used to express the pose (position and attitude) and misalignment, respectively. SE(3) contains position and attitude transformation matrices, and S3 comprises unit quaternions. The increments in pose and misalignment are represented in the Lie algebra, which is a linear space. The use of Lie algebra facilitates the application of an extended Kalman filter (EKF). The previous EKF approach without Lie theory is based on the assumption that a non-differentiable space can be approximated as a differentiable space when the increments are sufficiently small. On the contrary, the proposed Lie theory approach enables exact differentiation in a differentiable space, thus enhances the accuracy of the navigation. Furthermore, the convergence and stability of the internal parameters, such as the Kalman gain and measurement innovation, are improved.

Three-Dimensional Dead-Reckoning Based on Lie Theory for Overcoming Approximation Errors

“…The concept of a smooth manifold is related to differential geometry, and the smoothness of a Lie group means that the operation between transformations is differentiable. Commonly used Lie groups in robotics include the n-dimensional special orthogonal group (SO(n)) [28], which represents rotation transformations, the n-dimensional special unitary group (SU(n)), which plays an important role in quantum mechanics, and the special Euclidean group (SE(3)), which represents rotations and translations in three-dimensional space [29][30][31]. In addition, there is SE n (3) extended from SE(3) for rigid body transformations related to transformations or kinematic changes [26,[32][33][34].…”