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 ((Formula presented.)) and 3-sphere space ((Formula presented.)) are used to express the pose (position and attitude) and misalignment, respectively. (Formula presented.) contains position and attitude transformation matrices, and (Formula presented.) 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.