| Summary: | 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 (<inline-formula><math xmlns="http://www.w3.org/1998/Math/MathML" display="inline"><semantics><mrow><mi mathvariant="italic">SE</mi><mo>(</mo><mn>3</mn><mo>)</mo></mrow></semantics></math></inline-formula>) and 3-sphere space (<inline-formula><math xmlns="http://www.w3.org/1998/Math/MathML" display="inline"><semantics><msup><mi mathvariant="italic">S</mi><mn>3</mn></msup></semantics></math></inline-formula>) are used to express the pose (position and attitude) and misalignment, respectively. <inline-formula><math xmlns="http://www.w3.org/1998/Math/MathML" display="inline"><semantics><mrow><mi mathvariant="italic">SE</mi><mo>(</mo><mn>3</mn><mo>)</mo></mrow></semantics></math></inline-formula> contains position and attitude transformation matrices, and <inline-formula><math xmlns="http://www.w3.org/1998/Math/MathML" display="inline"><semantics><msup><mi mathvariant="italic">S</mi><mn>3</mn></msup></semantics></math></inline-formula> 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.
|
|---|