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 ($\mathit{SE}\left(3\right)$) and 3-sphere space (${\mathit{S}}^3$) are used to express the pose (position and attitude) and misalignment, respectively. $\mathit{SE}\left(3\right)$ contains position and attitude transformation matrices, and ${\mathit{S}}^3$ 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.

1. Introduction

Sensing and monitoring in an underwater environment rely heavily on the reliable navigation of an underwater vehicle. The underwater vehicles installed with sensors for environmental monitoring can navigate through sites of interest and accumulate the sensor data for analysis. The inertial navigation system (INS) and Doppler velocity log (DVL) play pivotal roles in ensuring accurate underwater navigation [1,2,3,4,5]. Misalignment between the INS and DVL results in a degradation of overall navigation performance. This misalignment leads to persistent challenges in achieving successful navigation.

This paper proposes a method that estimates and compensates for the misalignment between INS and DVL in underwater vehicles. The proposed method incorporates Lie theory into Kalman filter (KF) to estimate explicitly the misalignment as well as the navigation variables such as the position and attitude for which the misalignment is compensated. Arithmetic operations based on the assumption that misalignment variables constitute linear space require approximations and cause instability in estimation. This limitation calls for Lie theory for rigorous implementation of the estimation. The use of Lie theory makes the estimation more accurate and stable.

INS and DVL are generally used as sensors for the navigation of underwater vehicles. In [1], for robust INS/DVL navigation, the complexity of the underwater environment is evaluated, and the navigation filters are adaptively employed depending on the complexity [1]. A new approach that combines factor graph optimization (FGO) and the maximum entropy criterion is proposed. This method replaces conventional nonlinear filters, whose performance degrades in the case of non-Gaussian measurement noise [2]. It also incorporates USBL into INS/DVL navigation. Another approach that also uses FGO and fuses USBL, INS, and DVL is proposed [3]. In [3], a robust M-estimation algorithm is incorporated into FGO. To improve INS/DVL integrated navigation performance when the measurements are corrupted by heavy-tailed non-Gaussian noise, a method that incorporates the error state Kalman filter (ESKF) with the interactive multiple model (IMM) is proposed [5]. Many of the previous methods concentrate on fusing the sensors; however, they often ignore the misalignment of sensors.

The misalignment of INS and DVL indicates the rotation of the DVL coordinate frame relative to the INS coordinate frame. In INS/DVL fused navigation, the velocity measured by the DVL is converted to the velocity in the INS coordinate frame using the rotation data of the DVL coordinate frame relative to the INS coordinate frame. If the coordinate frames of the INS and DVL are the same, the velocity measured by the DVL is identical to the velocity relative to the INS coordinate frame Any unknown misalignment between these two coordinate frames causes velocity errors, resulting in navigation errors. The degraded navigation performance affects the operational accuracy and stability of underwater vehicles during missions. Additionally, in a complex underwater environment with disturbances such as currents, waves, and tidal changes, the sensor output is affected to a greater degree; thus, the error increases further, and INS/DVL fused navigation deteriorates. In our research, rotation and attitude have the same kind of data representation and will be used interchangeably in the context of arithmetic operations.

Several approaches to evaluate the misalignment of sensors have been proposed. Ref. [6] evaluated the misalignment using the apparent velocity model and KF. Ref. [7] also estimates the misalignment using KF based on an approximated linear model. Ref. [4] proposed the arctangent fading memorial sage-husa adaptive Kalman filter (SHAKF) approach for evaluating misalignment. Ref. [8] estimated the misalignment using ESKF and incorporated Lie theory. In deriving the ESKF, attitude error is approximated linearly [8]. These approaches determine the misalignment before the operation of the underwater vehicle and are not applicable to online real-time operation They do not provide simultaneous navigation results. In addition, the nonlinear nature of the rotation is not fully addressed, even when Lie theory is employed. These methods do not handle operations on attitude or rotation rigorously, thus they result in slow convergence and do not guarantee convergence of the estimation depending on the circumstances.

The arithmetic operations on the attitude and misalignment, including differentiation, integration, and covariance calculations required to implement KF methodology are not trivial since attitude does not belong in a linear space [9,10,11]. Attitude and misalignment belong to nonlinear spaces, such as the 3-dimensional special orthogonal group $\mathit{SO}\left(3\right)$ and 3-sphere space ${\mathit{S}}^3$ [12,13]. The research [14,15] used usual operations on attitude assuming that linear approximation does not cause a big problem. Instead, they applied additional operations, such as normalization for approximation [16] or optimization of attitude using the Levenberg-Marquardt algorithm [17], followed by operations on the attitude. However, these approximations eventually impair the reliability and stability of the estimation. To solve this problem, we incorporate Lie theory into a KF to estimate the misalignment between sensors as well as the pose(position and attitude) of an underwater vehicle. Specifically, the extended KF (EKF) is implemented in the Lie groups of 3-dimensional special Euclidean group $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$.

The Lie-theory-based approach can also be applied to the particle filter (PF), cubature KF (CKF), and unscented KF (UKF) [18,19,20,21,22]. To incorporate Lie theory into PF, CKF, and UKF, mathematical transformations are required to render the approach compatible with Lie theory. The mathematical transformations involve additional calculations, which increase the computational load and time in the estimation process. The additional calculations are mostly for exponential and logarithmic functions for the addition and subtraction of particles and sigma points [18,20]. A method utilizing an optimal regression matrix was proposed to reduce the computational complexity [21].

Recently, several researchers employed Lie theory explicitly or implicitly for KF applications for underwater navigation [8,23,24]. Lie theory combines strap-down INS (SINS) and DVL to solve the initial alignment problem for underwater vehicle navigation [8]. Another method, the Invariant Extended Kalman Filter (IEKF), extends the EKF by incorporating the characteristics of error dynamics, which are represented by a log-linear differential equation for underwater vehicle navigation [23]. A Lie group EKF is also beneficial for the heading initialization of autonomous underwater robots in situations with magnetic disturbance [24]. Whereas previous studies utilizing Lie theory approaches employed a theoretical approach using only a single Lie group space, our research incorporates two Lie groups $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$ into a single state vector. This makes it possible to estimate both the pose of the vehicle and the misalignment of sensors simultaneously.

One of the new features of this research is the use of Lie theory for the simultaneous estimation of the position and attitude of a vehicle as well as the misalignment. Also, the use of the two Lie groups in one state vector is new feature of this research. The use of the Lie theory facilitates the differentiation of the process model and measurement model to derive the Jacobian matrices, which are essential for EKF implementation. The proposed method follows the principle of IEKF and the stability and convergence of the approach is well established [25]. Though the Lie theory based implementation is not trivial unlike the Kalman filter implementation in a linear space, the performance of the methodology is proved in industrial implementations [26]. It results in accurate and consistent estimation which is robust against disturbances [27].

The novelty and contributions of the proposed approach are summarized as follows:

• A method for numerically calculating Jacobians using Lie theory is introduced. This approach alleviates the need for analytical derivation of Jacobians, simplifying the implementation of the Kalman filter, especially in cases where the process and measurement models are complex.
• It proposes an estimation method that uses state variables that belong in different Lie groups. It demonstrates that in cases where diverse Lie groups are combined for state variables, the Lie theory applies coherently, provided that operations corresponding to each Lie group are consistently employed.
• To the best of our knowledge, this research is the first to propose the use of Lie theory for the explicit estimation of misalignment, along with navigation variables such as position, attitude, and velocity. The application of Lie theory enhances estimation performance by reducing the estimation error and improving the convergence of the Kalman gain and measurement innovation.
• Two different Lie group elements in $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$ are incorporated into a state vector. This enables all the operations on the state variables and measurement variables to be rigorous without approximation. This proves that even though different Lie groups have different Lie algebras, they can be treated by a unified approach and simultaneously by Lie theory.
• The procedure to find the Jacobian matrices of the process model and measurement model in the Lie group is described in detail, thus it shows how the Lie theory is applied in physical implementation.
• This paper demonstrates the details involved in the implementation, and provides easy-to-follow guidance on the use of Lie theory for estimation.

The remainder of this paper is organized as follows. Section 2 describes the process and measurement models of the estimator based on the Lie theory. Section 3 presents the implementation of the EKF in the framework of Lie theory, using the Lie groups $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$. It includes the derivation of Jacobian matrices utilizing proper Lie theory. Section 4 describes the simulation results, and verifies the improvements made by the proposed method through comparisons with two other methods. One of the other methods neither uses Lie theory nor compensates for the misalignment. The second method estimates and compensates for the misalignment, but does not use Lie theory. Section 5 verifies the proposed method through field experiments that use a remotely operated vehicle (ROV). Section 6 concludes the paper and suggests directions for future research.

2. Process and Measurement Models Using the Lie Theory

This section begins with a definition of misalignment, a parameter that will be estimated. Then, essential tools from Lie theory, commonly used in applications to the KF, will be provided. Finally, the models will be derived using these tools.

2.1. Misalignment

In this study, the position and attitude of the underwater vehicle were considered to be the position and attitude of the INS. This indicates that the INS coordinate frame coincides with the coordinate frame of the underwater vehicle. The attitude of DVL is assumed to be the same as the INS. Therefore, misalignment indicates the rotation of DVL relative to the INS as shown in Figure 1.

The assumption that the DVL coordinate frame is the same as the INS coordinate frame can be applied generally even for the cases where the DVL is mounted with a predefined rotation relative to the INS coordinate frame. The predefined rotation can transform the velocity measured by the DVL to the velocity of the virtual DVL whose coordinate frame coincides with the INS coordinate frame, in compliance with the assumption. In this case, misalignment refers to the misalignment between the actual rotation and the predefined rotation.

2.2. Lie Theory Tools for KF

The position and attitude of an underwater vehicle compose the Lie group $\mathit{SE}\left(3\right)$ and the unit quaternions representing the misalignment compose the Lie group ${\mathit{S}}^3$ [11]. The increment and difference of Lie group elements in $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$ belong in Lie algebra $\mathfrak{se}\left(3\right)$ and ${\mathfrak{s}}^3$ respectively. The Lie algebra $\mathfrak{se}\left(3\right)$ and ${\mathfrak{s}}^3$ correspond to the Lie group $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$, respectively. Lie theory uses exponential function and logarithmic function to calculate the difference between two Lie group elements and to incorporate increment to a Lie group element. In addition, differentiation and integration are defined by using the exponential function and logarithmic function. Therefore, Lie theory is vital for the implementation of KF in the following stages: state prediction by integration, calculation of measurement innovation, representation of covariance matrix, correction of predicted state by incorporating the measurement innovation, and so on. The Lie theory approach enables the computation of the state error, noise, and increments without linear approximation.

2.2.1. Exponential Function

Let $\mathcal{M}$ be a Lie group and $\mathfrak{m}$ be the Lie algebra corresponding to the Lie group $\mathcal{M}$. The function $\mathrm{Exp}\left(\mathit{\tau }\right)$ maps a Lie algebra element $\mathit{\tau }\in \mathfrak{m}$ to a Lie group element $\mathit{\chi }\in \mathcal{M}$. $\mathrm{Exp}\left(\mathit{\tau }\right)$ differs from group to group [11]. If $\mathit{\tau }\in \mathfrak{se}\left(3\right)$, then $\mathrm{Exp}\left(\mathit{\tau }\right)$ is as follows.

$$\begin{array}{cc}\hfill & \mathrm{Exp}\left(\mathit{\tau }\right)=\left[\begin{array}{cc}\mathrm{Exp}\left(\mathit{\theta }\right)& \mathbf{V}\left(\mathit{\theta }\right)\mathit{\rho }\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in \mathit{SE}\left(3\right),\mathit{\tau }=\left[\begin{array}{c}\mathit{\rho }\\ \mathit{\theta }\end{array}\right]\in \mathfrak{se}\left(3\right).\hfill \end{array}$$

(1)

$\mathbf{V}\left(\mathit{\theta }\right)$ and $\mathrm{Exp}\left(\mathit{\theta }\right)$ are calculated as

$$\begin{array}{cc}\hfill & \mathbf{V}\left(\mathit{\theta }\right)=\mathbf{I}+\frac{1-cos\left(\theta \right)}{\theta }{\left[\mathbf{u}\right]}_{\times }+\frac{\theta -sin\left(\theta \right)}{\theta }{\left[\mathbf{u}\right]}_{\times }^2,\hfill \\ \hfill & \mathrm{Exp}\left(\mathit{\theta }\right)=\mathbf{I}+sin\left(\theta \right){\left[\mathbf{u}\right]}_{\times }+(1-cos\left(\theta \right)){\left[\mathbf{u}\right]}_{\times }^2,\hfill \end{array}$$

(2)

where, $\mathit{\theta }=\theta \mathbf{u},\parallel \mathbf{u}\parallel =1$, and ${\left[\begin{array}{c}·\end{array}\right]}_{\times }$ is an operator that transforms a vector into a skew-symmetric matrix.

If $\mathit{\tau }\in {\mathfrak{s}}^3$, then $\mathrm{Exp}\left(\mathit{\tau }\right)$ is as follows.

$$\begin{array}{cc}\hfill & \mathrm{Exp}\left(\mathit{\tau }\right)=\left[\begin{array}{c}cos\left({\displaystyle \frac{\theta }{2}}\right)\\ \mathbf{u}·sin\left({\displaystyle \frac{\theta }{2}}\right)\end{array}\right]\in {\mathit{S}}^3,\mathit{\tau }=\theta \mathbf{u}\in {\mathfrak{s}}^3,\parallel \mathbf{u}\parallel =1.\hfill \end{array}$$

(3)

2.2.2. Logarithmic Function

The function $\mathrm{Log}\left(\mathit{\chi }\right)$ maps a Lie group element $\mathit{\chi }\in \mathcal{M}$ to a Lie algebra element $\mathit{\tau }\in \mathfrak{m}$. It is the inverse map of $\mathrm{Exp}\left(\mathit{\tau }\right)$. $\mathrm{Log}\left(\mathit{\chi }\right)$ differs from group to group [11]. If $\mathit{\chi }\in \mathit{SE}\left(3\right)$, then $\mathrm{Log}\left(\mathit{\chi }\right)$ is as follows.

$$\mathrm{Log}\left(\mathit{\chi }\right)=\left[\begin{array}{c}{\mathbf{V}}^{-1}\left(\mathit{\theta }\right)\mathbf{p}\\ \mathrm{Log}\left(\mathbf{R}\right)\end{array}\right]\in \mathfrak{se}\left(3\right),\mathit{\chi }=\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in \mathit{SE}\left(3\right).$$

(4)

In Equation (4), $\mathrm{Log}\left(\mathbf{R}\right)$ and $\mathit{\theta }$ satisfies the followings:

$$\mathrm{Log}\left(\mathbf{R}\right)=\mathit{\theta }=\theta \mathbf{u}$$

(5)

$$\theta =arccos\left({\displaystyle \frac{\mathrm{trace}\left(\mathbf{R}\right)-1}{2}}\right)$$

(6)

$$\mathbf{u}={\displaystyle \frac{{(\mathbf{R}-{\mathbf{R}}^T)}^{\vee }}{2sin\left(\theta \right)}}.$$

(7)

In Equation (7), ${(·)}^{\vee }$ is an operator that transforms a skew-symmetric matrix into a vector [10,11].

If $\mathit{\chi }\in {\mathit{S}}^3$, then $\mathrm{Log}\left(\mathit{\chi }\right)$ is as follows.

$$\mathrm{Log}\left(\mathit{\chi }\right)=\mathit{\theta }=\theta \mathbf{u}\in {\mathfrak{s}}^3,\mathit{\chi }={\left[\begin{array}{cccc}{\mathrm{q}}_w& {\mathrm{q}}_x& {\mathrm{q}}_y& {\mathrm{q}}_z\end{array}\right]}^T\in {\mathit{S}}^3,$$

(8)

where,

$$\begin{array}{cc}\hfill \mathit{\chi }& ={\left[\begin{array}{cccc}{\mathrm{q}}_w& {\mathrm{q}}_x& {\mathrm{q}}_y& {\mathrm{q}}_z\end{array}\right]}^T={\left[\begin{array}{cc}{\mathrm{q}}_w& {\mathbf{q}}_v\end{array}\right]}^T\hfill \\ \hfill \theta & =2·\mathrm{atan}2(\parallel {\mathbf{q}}_v\parallel ,{\mathrm{q}}_w)\hfill \\ \hfill \mathbf{u}& ={\displaystyle \frac{{\mathbf{q}}_v}{\parallel {\mathbf{q}}_v\parallel }}.\hfill \end{array}$$

(9)

2.2.3. Plus and Minus Operator

The operator ⊕ combines a Lie group element $\mathit{\chi }\in \mathcal{M}$ with a Lie algebra element $\mathit{\tau }\in \mathfrak{m}$, resulting in a Lie group element in $\mathcal{M}$. Conversely, the operator ⊖ calculates the difference between two Lie group elements ${\mathit{\chi }}_1$ and ${\mathit{\chi }}_2$ in a Lie group $\mathcal{M}$, yielding a Lie algebra element in $\mathfrak{m}$.

The operator ⊕ is derived using the function $\mathrm{Exp}(·)$ as follows.

$$\begin{array}{c}\hfill \mathit{\chi }\oplus \mathit{\tau }=\mathit{\chi }·\mathrm{Exp}\left(\mathit{\tau }\right)\in \mathcal{M}.\end{array}$$

(10)

The operator ⊖ is derived using the function $\mathrm{Log}(·)$ as follows.

$$\begin{array}{c}\hfill {\mathit{\chi }}_1\ominus {\mathit{\chi }}_2=\mathrm{Log}\left({\mathit{\chi }}_2^{-1}·{\mathit{\chi }}_1\right)\in \mathfrak{m}.\end{array}$$

(11)

These derivations hold regardless of the Lie group $\mathcal{M}$ and its corresponding Lie algebra $\mathfrak{m}$ pair $(\mathcal{M},\mathfrak{m})$, provided that the functions $\mathrm{Exp}(·)$ and $\mathrm{Log}(·)$ are used consistently depending on the pair $(\mathcal{M},\mathfrak{m})$ [10,11].

2.3. Process Model

The state vector $\mathbf{X}\left(t\right)$ at time t consists of the position and attitude $\mathbf{M}\left(t\right)$ of the vehicle and the misalignment ^mq(t) as follows:

$$\mathbf{X}\left(t\right)={\left[\begin{array}{cc}\mathbf{M}\left(t\right)& { }^m\mathbf{q}\left(t\right)\end{array}\right]}^T.$$

(12)

In Equation (12), $\mathbf{M}\left(t\right)$ is a 4 × 4 matrix that represents the position and attitude of an underwater vehicle which composes the Lie group $\mathit{SE}\left(3\right)$ as follows:

$$\mathbf{M}\left(t\right)=\left[\begin{array}{cc}\mathbf{R}\left(\mathbf{q}\right(t\left)\right)& \mathbf{p}\left(t\right)\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in \mathit{SE}\left(3\right).$$

(13)

In Equation (13), $\mathbf{p}\left(t\right)={\left[\begin{array}{ccc}x\left(t\right)& y\left(t\right)& z\left(t\right)\end{array}\right]}^T$ represents the position of the vehicle which belongs in three-dimensional real vector space. $\mathbf{R}\left(\mathbf{q}\right(t\left)\right)$ is the rotation matrix corresponding to the unit quaternion $\mathbf{q}\left(t\right)$. The position is described in the North-East-Down (NED) coordinate frame. $\mathbf{q}\left(t\right)={\left[\begin{array}{cccc}{\mathrm{q}}_w\left(t\right)& {\mathrm{q}}_x\left(t\right)& {\mathrm{q}}_y\left(t\right)& {\mathrm{q}}_z\left(t\right)\end{array}\right]}^T$ is the attitude of the underwater vehicle represented by a unit quaternion. In Equation (12), ^mq(t) is the misalignment, which is the rotation of the true DVL coordinate frame from the INS coordinate frame. Because the rotations constitute the Lie group ${\mathit{S}}^3$, ^mq(t) belongs to ${\mathit{S}}^3$.

$${ }^m\mathbf{q}\left(t\right)={\left[\begin{array}{cccc}{ }^m{\mathrm{q}}_w\left(t\right)& { }^m{\mathrm{q}}_x\left(t\right)& { }^m{\mathrm{q}}_y\left(t\right)& { }^m{\mathrm{q}}_z\left(t\right)\end{array}\right]}^T\in {\mathit{S}}^3.$$

(14)

To describe both the attitude and position in one frame, $\mathit{SE}\left(3\right)$ is used. $\mathit{SE}\left(3\right)$ contains the Lie group $\mathit{SO}\left(3\right)$ to describe the attitude. The attitude of the vehicle $\mathbf{q}\left(t\right)$ converts to the rotation matrix in $\mathit{SO}\left(3\right)$ as the following.

$$\begin{array}{ccc}\hfill & \mathbf{R}\left(\mathbf{q}\right(t\left)\right)=\hfill & \hfill \left[\begin{array}{ccc}1-2{\mathrm{q}}_y^2\left(t\right)-2{\mathrm{q}}_z^2\left(t\right)& 2{\mathrm{q}}_x\left(t\right){\mathrm{q}}_y\left(t\right)-2{\mathrm{q}}_z\left(t\right){\mathrm{q}}_w\left(t\right)& 2{\mathrm{q}}_x\left(t\right){\mathrm{q}}_z\left(t\right)+2{\mathrm{q}}_y\left(t\right){\mathrm{q}}_w\left(t\right)\\ 2{\mathrm{q}}_x\left(t\right){\mathrm{q}}_y\left(t\right)+2{\mathrm{q}}_z\left(t\right){\mathrm{q}}_w\left(t\right)& 1-2{\mathrm{q}}_x^2\left(t\right)-2{\mathrm{q}}_z^2\left(t\right)& 2{\mathrm{q}}_y\left(t\right){\mathrm{q}}_z\left(t\right)-2{\mathrm{q}}_x\left(t\right){\mathrm{q}}_w\left(t\right)\\ 2{\mathrm{q}}_x\left(t\right){\mathrm{q}}_z\left(t\right)-2{\mathrm{q}}_y\left(t\right){\mathrm{q}}_w\left(t\right)& 2{\mathrm{q}}_x\left(t\right){\mathrm{q}}_w\left(t\right)+2{\mathrm{q}}_y\left(t\right){\mathrm{q}}_z\left(t\right)& 1-2{\mathrm{q}}_x^2\left(t\right)-2{\mathrm{q}}_y^2\left(t\right)\end{array}\right].\end{array}$$

(15)

The process model describes state $\mathbf{X}\left(t_{k+1}\right)$ as a function of state $\mathbf{X}\left(t_k\right)$ and the measured input $\mathit{U}\left(t_k\right)$ as follows:

$$\begin{array}{cc}\hfill & \mathbf{X}\left(t_{k+1}\right)=\mathit{f}(\mathbf{X}\left(t_k\right),\mathit{U}\left(t_k\right))+\mathbf{w},\mathbf{w}\sim \mathcal{N}(0,\mathit{Q}),\hfill \end{array}$$

(16)

where $\mathbf{w}$ is the process noise. $\mathit{U}\left(t_k\right)$ is the measured input which is expressed as follows:

$$\mathit{U}\left(t_k\right)=\left[\begin{array}{c}\mathbf{v}\left(t_k\right)\\ \mathit{\omega }\left(t_k\right)\end{array}\right].$$

(17)

In Equation (17), $\mathbf{v}\left(t_k\right)={\left[\begin{array}{ccc}u\left(t_k\right)& v\left(t_k\right)& w\left(t_k\right)\end{array}\right]}^T$ is the linear velocity of the vehicle. $\mathit{\omega }\left(t_k\right)={\left[\begin{array}{ccc}p\left(t_k\right)& q\left(t_k\right)& r\left(t_k\right)\end{array}\right]}^T$ is the angular velocity measured by INS.

Lie theory provides the process model for the location and attitude $\mathbf{M}\left(t_k\right)$ of the vehicle as Equation (18).

$$\begin{array}{cc}\hfill \mathbf{M}\left(t_{k+1}\right)& =\mathbf{M}\left(t_k\right)\oplus (\mathit{U}\left(t_k\right)·▵t_{k+1}),\hfill \end{array}$$

(18)

where $▵t_{k+1}$ denotes $t_{k+1}-t_k$. The operator ⊕ which is implemented by using the exponential function adds a Lie algebra element to a Lie group element.

The measured velocity ^Dv(t_k) should be transformed to the velocity $\mathbf{v}\left(t_k\right)$ in the INS coordinate frame using the misalignment ${ }^m\mathbf{q}\left(t_k\right)$, as follows:

$$\mathbf{v}\left(t_k\right)={ }^m\mathbf{q}\left(t_k\right)\otimes { }^D\mathbf{v}\left(t_k\right)\otimes { }^m{\mathbf{q}}^{*}\left(t_k\right),$$

(19)

where ${ }^D\mathbf{v}\left(t_k\right)$ is the linear velocity measured by the DVL and ⊗ is the operator that multiplies two quaternions [10].

In Equation (10), $\mathbf{M}\left(t_k\right)$ is in Lie group $\mathit{SE}\left(3\right)$ and $\mathit{U}\left(t_k\right)·▵t_{k+1}$ is in Lie algebra $\mathfrak{se}\left(3\right)$ which is associated with $\mathit{SE}\left(3\right)$. $\mathit{U}\left(t_k\right)·▵t_{k+1}$ indicates changes in the 6 degrees of freedom motion and is represented as a six-dimensional vector in ${\mathbb{R}}^6$.

$$\begin{array}{c}\hfill \mathit{U}\left(t_k\right)·▵t_{k+1}=\left[\begin{array}{c}▵t_{k+1}·\mathbf{v}\left(t_k\right)\\ ▵t_{k+1}·\mathit{\omega }\left(t_k\right)\end{array}\right]=\left[\begin{array}{c}\mathit{\rho }\\ \mathit{\theta }\end{array}\right]\in {\mathbb{R}}^6.\end{array}$$

(20)

The misalignment is assumed to be constant over time because the sensors are mechanically fixed to the underwater vehicle. Therefore, the process model for the misalignment ${ }^m\mathbf{q}\left(t_k\right)$ is as follows:

$${ }^m\mathbf{q}\left(t_{k+1}\right)={ }^m\mathbf{q}\left(t_k\right).$$

(21)

The function $\mathit{f}(\mathbf{X}\left(t_k\right),\mathit{U}\left(t_k\right))$ in the process model of Equation (16) for EKF implementation comprises Equations (18) and (21). Equation (18), together with Equation (19), compensates for misalignment and updates the state. By utilizing Lie theory, exact state prediction without approximation errors is made possible.

2.4. Measurement Model

The position of the underwater vehicle measured by the Ultra-Short Baseline (USBL) and the attitude by the INS are used as the measurements for the estimation [28]. The measurement vector $\mathbf{Z}\left(t\right)$ is represented as a Lie group comprising position and attitude as follows:

$$\mathbf{Z}\left(t\right)=\left[\begin{array}{cc}\mathbf{R}\left(\tilde{\mathbf{q}}\left(t\right)\right)& \tilde{\mathbf{p}}\left(t\right)\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\in \mathit{SE}\left(3\right),$$

(22)

$$\mathrm{where},\tilde{\mathbf{p}}\left(\mathrm{t}\right)=\left[\begin{array}{c}\tilde{\mathrm{x}}\left(\mathrm{t}\right)\\ \tilde{\mathrm{y}}\left(\mathrm{t}\right)\\ \tilde{\mathrm{z}}\left(\mathrm{t}\right)\end{array}\right];\tilde{\mathbf{q}}\left(\mathrm{t}\right)=\left[\begin{array}{c}{\tilde{\mathrm{q}}}_{\mathrm{w}}\left(\mathrm{t}\right)\\ {\tilde{\mathrm{q}}}_{\mathrm{x}}\left(\mathrm{t}\right)\\ {\tilde{\mathrm{q}}}_{\mathrm{y}}\left(\mathrm{t}\right)\\ {\tilde{\mathrm{q}}}_{\mathrm{z}}\left(\mathrm{t}\right)\end{array}\right].$$

(23)

In Equation (22), $\tilde{\mathbf{p}}\left(t\right)$ is the position measured by the USBL and $\tilde{\mathbf{q}}\left(t\right)$ is the attitude measured by INS. The rotation matrix $\mathbf{R}\left(\tilde{\mathbf{q}}\left(t\right)\right)\in SO\left(3\right)$ is computed using Equation (15). The measurements $\mathit{Z}\left(t\right)$ constitute the Lie group SE(3).

The measurement model is a function which relates the state $\mathbf{X}\left(t_k\right)$ to the measurement $\mathit{Z}\left(t_k\right)$, as follows:

$$\begin{array}{cc}\hfill \mathit{Z}\left(t_k\right)=& \mathit{h}\left(\mathbf{X}\left(t_k\right)\right)+\mathbf{n}\hfill \\ \hfill =& \mathbf{M}\left(t_k\right)+\mathbf{n},\mathbf{n}\sim \mathcal{N}(0,\mathit{R}),\hfill \end{array}$$

(24)

where $\mathbf{n}$ is the measurement noise, which is a Gaussian distributed random variable with a mean of zero and covariance $\mathit{R}$.

3. Use of Lie Theory for Estimation in Lie Groups $\mathit{SE}\left(\mathbf{3}\right)$ and ${\mathit{S}}^{\mathbf{3}}$

Lie theory is needed for operations on states and measurements which are required for implementation of EKF. The process model and measurement model are linearized to Jacobian matrices by partial differentiation of the models. The partial differentiation requires Lie theory. Also, adding increments to the states and calculating the difference between state variables requires Lie theory. If Lie theory is not utilized, the operations involve errors, and this deteriorates the estimation accuracy and convergence performance. This section explains the use of Lie theory for the derivation of Jacobian matrices and arithmetic operations.

3.1. Jacobian of Process Model Relative to State Variables

The Jacobian matrix of the process model Equations (18) and (21) with respect to state variables is given by

$$\begin{array}{cc}\hfill \mathbf{A}\left(t_{k+1}\right)& =\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{\partial \mathbf{M}\left(t_k\right)}}& {\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{{\partial }^m\mathbf{q}\left(t_k\right)}}\\ {\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathbf{M}\left(t_k\right)}}& {\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{{\partial }^m\mathbf{q}\left(t_k\right)}}\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{cccc}{\mathbf{A}}_1\left(t_{k+1}\right)& {\mathbf{A}}_2\left(t_{k+1}\right)& \cdots & {\mathbf{A}}_9\left(t_{k+1}\right)\end{array}\right],\hfill \end{array}$$

(25)

where $\mathbf{A}\left(t_{k+1}\right)$ has the size of 9 × 9. $\mathbf{A}\left(t_{k+1}\right)$ is numerically computed through the finite difference method based on the Lie theory as

$$\begin{array}{cc}\hfill {\mathbf{A}}_i\left(t_{k+1}\right)& \simeq {\displaystyle \frac{\mathit{f}(\mathbf{X}\left(t_k\right)\oplus {\delta }_i,\mathit{U}\left(t_k\right))\ominus \mathit{f}(\mathbf{X}\left(t_k\right),\mathit{U}\left(t_k\right))}{\delta }},\hfill \end{array}$$

(26)

where the operator ⊖ returns a Lie algebra element which represents the difference between two Lie group elements. $\delta$ should be set to a sufficiently small value, and it is taken into the vector ${\delta }_i$ when the operation is performed as many times as the sum of the dimension of the Lie algebras $\mathfrak{se}\left(3\right)$ and ${\mathfrak{s}}^3$.

$${\mathit{\delta }}_1=\left[\begin{array}{c}\delta \\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right],{\mathit{\delta }}_2=\left[\begin{array}{c}0\\ \delta \\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right],\cdots ,{\mathit{\delta }}_9=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ \delta \end{array}\right].$$

(27)

The calculation of $\mathbf{A}\left(t_{k+1}\right)$ requires different procedures depending on the Lie groups $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$. This is because the state $\mathbf{X}\left(t_k\right)$ contains both the elements of $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$. Although the element of $\mathit{SE}\left(3\right)$ has $4\times 4$ components, the increments form a 6-dimensional tangent space $\mathfrak{se}\left(3\right)$. Likewise, while the unit quaternions in ${\mathit{S}}^3$ has four components, its increments are in a 3-dimensional tangent space ${\mathfrak{s}}^3$.

${\mathbf{A}}_i\left(t_{k+1}\right)$(1:6) which is part of ${\mathbf{A}}_i\left(t_{k+1}\right)$ is calculated as follows:

$$\begin{array}{cc}\hfill & {\mathbf{A}}_i\left(t_{k+1}\right)(1:6)=\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{\partial \mathbf{M}\left(t_k\right)}}& {\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{{\partial }^m\mathbf{q}\left(t_k\right)}}\end{array}\right]\hfill \\ \hfill & \simeq {\displaystyle \frac{\mathit{f}(\mathbf{M}\left(t_k\right)\oplus {\delta }_i(1:6),\mathit{U}\left(t_k\right))\ominus \mathit{f}(\mathbf{M}\left(t_k\right),\mathit{U}\left(t_k\right))}{\delta }}.\hfill \end{array}$$

(28)

The vector ${\delta }_i$(1:6) which comprises the first 6 rows of ${\delta }_i$ is the vector corresponding to an element in the Lie algebra $\mathfrak{se}\left(3\right)$. Expanding $\mathbf{M}\left(t_k\right)\oplus {\delta }_i$(1:6) yields $\mathbf{M}\left(t_k\right)$·$\mathrm{Exp}$ (${\delta }_i$(1:6)). We employ Equation (1) for the computation of $\mathrm{Exp}$(·) function.

Next, the remaining part of the Jacobian of ${\mathbf{A}}_i\left(t_{k+1}\right)$ is calculated as follows:

$$\begin{array}{cc}\hfill & {\mathbf{A}}_i\left(t_{k+1}\right)(7:9)=\left[\begin{array}{cc}{\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathbf{M}\left(t_k\right)}}& {\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{{\partial }^m\mathbf{q}\left(t_k\right)}}\end{array}\right]\hfill \\ \hfill & \simeq {\displaystyle \frac{\mathit{f}\left({ }^m\mathbf{q}\left(t_k\right)\oplus {\delta }_i(7:9),\mathit{U}\left(t_k\right)\right)\ominus \mathit{f}({ }^m\mathbf{q}\left(t_k\right),\mathit{U}\left(t_k\right))}{\delta }}.\hfill \end{array}$$

(29)

In Equation (29), ${\delta }_i(7:9)$ which comprises rows (7:9) of ${\delta }_i$ is the tangent vector in Lie algebra ${\mathfrak{s}}^3$. Expanding ${ }^m\mathbf{q}\left(t_k\right)\oplus {\delta }_i$(7:9) yields ${ }^m\mathbf{q}\left(t_k\right)$⊗$\mathrm{Exp}$(${\delta }_i$(7:9)).

The Jacobian matrix ${\mathbf{A}}_i\left(t_{k+1}\right)(7:9)$ can be calculated numerically using Equation (29). Alternatively, ${\mathbf{A}}_i\left(t_{k+1}\right)(7:9)$ can be derived analytically as follows:

$${\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathbf{M}\left(t_k\right)}}={\mathbf{0}}_{3\times 6},{\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{{\partial }^m\mathbf{q}\left(t_k\right)}}={\mathbf{I}}_3.$$

(30)

3.2. Jacobian of Process Model with Respect to Measured Input Variables

The Jacobian matrix for Equation (17) with respect to the measured input is as follows:

$$\begin{array}{cc}\hfill { }^{\mathit{U}}\mathbf{A}\left(t_{k+1}\right)& =\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{\partial \mathbf{v}\left(t_k\right)}}& {\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{\partial \mathit{\omega }\left(t_k\right)}}\\ {\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathbf{v}\left(t_k\right)}}& {\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathit{\omega }\left(t_k\right)}}\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{cccc}{ }^{\mathit{U}}{\mathbf{A}}_1\left(t_{k+1}\right)& { }^{\mathit{U}}{\mathbf{A}}_2\left(t_{k+1}\right)& \cdots & { }^{\mathit{U}}{\mathbf{A}}_6\left(t_{k+1}\right)\end{array}\right],\hfill \end{array}$$

(31)

where ${ }^{\mathit{U}}\mathbf{A}\left(t_{k+1}\right)$ is a $9\times 6$ matrix. The first six rows of ${ }^{\mathit{U}}\mathbf{A}\left(t_{k+1}\right)$ can be calculated numeriacally by Equation (32).

$$\begin{array}{cc}\hfill { }^{\mathit{U}}{\mathbf{A}}_i\left(t_{k+1}\right)(1:6)& =\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{\partial \mathbf{v}\left(t_k\right)}}& {\displaystyle \frac{\partial \mathbf{M}\left(t_{k+1}\right)}{\partial \mathit{\omega }\left(t_k\right)}}\end{array}\right]\hfill \\ \hfill & \simeq {\displaystyle \frac{\mathit{f}(\mathbf{M}\left(t_k\right),\mathit{U}\left(t_k\right)+{ϵ}_i)\ominus \mathit{f}(\mathbf{M}\left(t_k\right),\mathit{U}\left(t_k\right))}{ϵ}}.\hfill \end{array}$$

(32)

Similar to ${\delta }_i$ and $\delta$ in Equation (26), $ϵ$ has a sufficiently small value, and it is taken into the vector ${ϵ}_i$ when the operation is performed 6 times.

$${\mathit{ϵ}}_1=\left[\begin{array}{c}ϵ\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right],{\mathit{ϵ}}_2=\left[\begin{array}{c}0\\ ϵ\\ 0\\ 0\\ 0\\ 0\end{array}\right],\cdots ,{\mathit{ϵ}}_6=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ ϵ\end{array}\right].$$

(33)

The last three rows of ${ }^{\mathit{U}}\mathbf{A}\left(t_{k+1}\right)$ are derived analytically as follows:

$${ }^{\mathit{U}}{\mathbf{A}}_i\left(t_{k+1}\right)(7:9)=\left[\begin{array}{cc}{\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathbf{v}\left(t_k\right)}}& {\displaystyle \frac{{\partial }^m\mathbf{q}\left(t_{k+1}\right)}{\partial \mathit{\omega }\left(t_k\right)}}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right].$$

(34)

Because the misalignment ${ }^m\mathbf{q}\left(t_{k+1}\right)$ does not depend on both $\mathbf{v}\left(t_k\right)$ and $\mathit{\omega }\left(t_k\right)$, the partial derivatives of ${ }^m\mathbf{q}\left(t_{k+1}\right)$ with respect to $\mathbf{v}\left(t_k\right)$ and $\mathit{\omega }\left(t_k\right)$ are zeros, as shown in Equation (34).

3.3. Jacobian of Measurement Model

The Jacobian matrix for the measurement model in Equation (24) is derived as follows:

$$\mathbf{H}\left(t_k\right)={\displaystyle \frac{\partial \mathit{h}\left(\mathbf{X}\left(t_k\right)\right)}{\partial \mathbf{X}\left(t_k\right)}}=\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}\left(t_k\right)}{\partial \mathbf{M}\left(t_k\right)}}& {\displaystyle \frac{\partial \mathbf{M}\left(t_k\right)}{{\partial }^m\mathbf{q}\left(t_k\right)}}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{I}}_6& {\mathbf{0}}_{6\times 3}\end{array}\right].$$

(35)

3.4. Lie Theory Based EKF Procedure

The EKF estimates position, attitude, and misalignment. It uses Jacobian matrices derived in Section 3.1, Section 3.2 and Section 3.3 using Lie theory. In addition to employing Jacobian matrices, the EKF uses arithmetic operations based on Lie theory.

The estimated state $\widehat{\mathbf{M}}\left(t_k\right)$ and ${ }^m\widehat{\mathbf{q}}\left(t_k\right)$ are propagated to the states $\widehat{\mathbf{M}}\left(t_{k+1}^{-}\right)$ and ${ }^m\widehat{\mathbf{q}}\left(t_{k+1}^{-}\right)$ for the time $t=t_{k+1}$ using the Equations (18) and (21), as follows:

$$\begin{array}{cc}\hfill \widehat{\mathbf{M}}\left(t_{k+1}^{-}\right)& =\widehat{\mathbf{M}}\left(t_k\right)·\mathrm{Exp}(\mathit{U}\left(t_k\right)·▵t_{k+1})\hfill \\ \hfill { }^m\widehat{\mathbf{q}}\left(t_{k+1}^{-}\right)& ={ }^m\widehat{\mathbf{q}}\left(t_k\right).\hfill \end{array}$$

(36)

In Equation (36), $\mathrm{Exp}(\mathit{U}\left(t_k\right)·▵t_{k+1})$ for Lie group $\mathit{SE}\left(3\right)$ is calculated using Equation (1). The Lie group exponential operation guarantees accurate prediction. In contrast, the numerical integration of the method [29] causes errors in the predicted position and attitude.

The covariance $\mathbf{P}\left(t_{k+1}^{-}\right)$ of the predicted state $\widehat{\mathbf{X}}\left(t_{k+1}^{-}\right)={\left[\begin{array}{cc}\widehat{\mathbf{M}}\left(t_{k+1}^{-}\right)& { }^m\widehat{\mathbf{q}}\left(t_{k+1}^{-}\right)\end{array}\right]}^T$ is computed using the Jacobian matrices $\mathbf{A}$ and ${ }^{\mathit{U}}\mathbf{A}$ as follows:

$$\begin{array}{cc}\hfill \mathbf{P}\left(t_{k+1}^{-}\right)=& \mathbf{A}\left(t_{k+1}\right)·\mathbf{P}\left(t_k\right)·{\mathbf{A}}^T\left(t_{k+1}\right)+{ }^{\mathit{U}}\mathbf{A}\left(t_{k+1}\right)·{ }^{\mathit{U}}\mathit{Q}·{ }^{\mathit{U}}{\mathbf{A}}^T\left(t_{k+1}\right)+\mathit{Q}.\hfill \end{array}$$

(37)

The covariance matrices $\mathbf{P}\left(t_k\right)$ and $\mathbf{P}\left(t_{k+1}^{-}\right)$ have a dimension of $9\times 9$. This is a distinct characteristic of the Lie theory based EKF implementation compared with the EKF implementation by [29]. In [29], the covariance of the state has a dimension of $11\times 11$, because it regarded position as a variable in ${\mathbb{R}}^3$, attitude as a variable in ${\mathbb{R}}^4$, and misalignment as a variable in ${\mathbb{R}}^4$, not in Lie groups. Moreover, in [29], operations on attitude and misalignment do not provide proper attitude and misalignment.

Uncertainty in the measured input affects the estimation. This is considered by including ${ }^{\mathit{U}}\mathbf{A}\left(t_{k+1}\right)·{ }^{\mathit{U}}\mathit{Q}·{ }^{\mathit{U}}{\mathbf{A}}^T\left(t_{k+1}\right)$ in Equation (37). Here, ${ }^{\mathit{U}}\mathit{Q}$ is the covariance of the measured input and is a $6\times 6$ matrix.

According to Lie theory, the measurement innovation is computed by using $\mathrm{Log}(·)$ function of Lie group $\mathit{SE}\left(3\right)$ as follows:

$$\begin{array}{cc}\hfill \mathit{Y}\left(t_{k+1}\right)& =\mathit{Z}\left(t_{k+1}\right)\ominus \mathit{h}(\widehat{\mathbf{X}}\left(t_{k+1}^{-}\right),0)\hfill \\ \hfill & =\mathrm{Log}(\mathit{h}{(\widehat{\mathbf{X}}\left(t_{k+1}^{-}\right),0)}^{-1}·\mathit{Z}\left(t_{k+1}\right))\hfill \\ \hfill & =\mathrm{Log}({\widehat{\mathbf{M}}}^{-1}\left(t_{k+1}^{-}\right)·\mathit{Z}\left(t_{k+1}\right)).\hfill \end{array}$$

(38)

The $\mathrm{Log}(·)$ of the Lie group $\mathit{SE}\left(3\right)$ is given as Equation (4). The value of $\mathrm{Log}(·)$ belongs in Lie algebra $\mathfrak{se}\left(3\right)$.

The Kalman gain is computed as follows:

$$\begin{array}{cc}\hfill & \mathbf{K}\left(t_{k+1}\right)=\mathbf{P}\left(t_{k+1}^{-}\right)·\mathbf{H}{\left(t_{k+1}\right)}^T·{\mathit{S}}^{-1}\left(t_{k+1}\right)\hfill \\ \hfill & \mathrm{where},\mathit{S}\left(t_{k+1}\right)=\mathbf{H}\left(t_{k+1}\right)·\mathbf{P}\left(t_{k+1}^{-}\right)·{\mathbf{H}}^T\left(t_{k+1}\right)+\mathit{R}.\hfill \end{array}$$

(39)

The Kalman gain $\mathbf{K}\left(t_{k+1}\right)$ is a $9\times 6$ matrix.

Using Lie theory, the correction state $\delta \widehat{\mathbf{X}}\left(t_{k+1}\right)$ is added to the predicted state ${\widehat{\mathbf{X}}}^{-}\left(t_{k+1}\right)$ using the operator ⊕, to provide the corrected state $\widehat{\mathbf{X}}\left(t_{k+1}\right)$.

$$\begin{array}{cc}\hfill & \widehat{\mathbf{X}}\left(t_{k+1}\right)=\widehat{\mathbf{X}}\left(t_{k+1}^{-}\right)\oplus \delta \widehat{\mathbf{X}}\left(t_{k+1}\right)\hfill \\ \hfill & \mathrm{where},\delta \widehat{\mathbf{X}}\left(t_{k+1}\right)=\left[\begin{array}{c}\delta \widehat{\mathbf{M}}\left(t_{k+1}\right)\\ {\delta }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)\end{array}\right]=\mathbf{K}\left(t_{k+1}\right)·\mathit{Y}\hfill \end{array}$$

(40)

Equation (40) is rephrased as,

$$\begin{array}{c}\hfill \left[\begin{array}{c}\widehat{\mathbf{M}}\left(t_{k+1}\right)\\ { }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{c}\widehat{\mathbf{M}}\left(t_{k+1}^{-}\right)\\ { }^m\widehat{\mathbf{q}}\left(t_{k+1}^{-}\right)\end{array}\right]\oplus \left[\begin{array}{c}\delta \widehat{\mathbf{M}}\left(t_{k+1}\right)\\ {\delta }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)\end{array}\right]\end{array}$$

(41)

Lie theory implements Equation (41) using $\mathrm{Exp}(·)$ as follows:

$$\begin{array}{cc}\hfill \widehat{\mathbf{M}}\left(t_{k+1}\right)& =\widehat{\mathbf{M}}\left(t_{k+1}^{-}\right)\oplus \delta \widehat{\mathbf{M}}\left(t_{k+1}\right)=\widehat{\mathbf{M}}\left(t_{k+1}^{-}\right)·\mathrm{Exp}\left(\delta \widehat{\mathbf{M}}\left(t_{k+1}\right)\right)\hfill \\ \hfill { }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)& ={ }^m\widehat{\mathbf{q}}\left(t_{k+1}^{-}\right)\oplus {\delta }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)={ }^m\widehat{\mathbf{q}}\left(t_{k+1}^{-}\right)\otimes \mathrm{Exp}\left({\delta }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)\right).\hfill \end{array}$$

(42)

In Equation (42), $\mathrm{Exp}\left(\delta \widehat{\mathbf{M}}\left(t_{k+1}\right)\right)$ and $\mathrm{Exp}\left({\delta }^m\widehat{\mathbf{q}}\left(t_{k+1}\right)\right)$ are exponential functions in $\mathit{SE}\left(3\right)$ and ${\mathit{S}}^3$, respectively, as shown in Equations (1) and (3). The measurement update using Equation (42) is accurate without approximation. If Lie theory is not applied, addition and subtraction for measurement update leads to improper results and approximation errors.

Finally, the predicted covariance $\mathbf{P}\left(t_{k+1}^{-}\right)$ is updated to the covariance $\mathbf{P}\left(t_{k+1}\right)$ of the estimated state $\widehat{\mathbf{X}}\left(t_{k+1}\right)$ as follows:

$$\begin{array}{cc}\hfill \mathbf{P}\left(t_{k+1}\right)=& (\mathbf{I}-\mathbf{K}\left(t_{k+1}\right)·\mathbf{H}\left(t_{k+1}\right))·\mathbf{P}\left(t_{k+1}^{-}\right)·{(\mathbf{I}-\mathbf{K}\left(t_{k+1}\right)·\mathbf{H}\left(t_{k+1}\right))}^T\hfill \\ \hfill & +\mathbf{K}\left(t_{k+1}\right)·\mathit{R}·{\mathbf{K}}^T\left(t_{k+1}\right).\hfill \end{array}$$

(43)

Equation (43) uses the Jacobian matrix $\mathbf{H}\left(t_{k+1}\right)$ which is derived using Lie theory as shown in Equation (35).

4. Simulation Test and Results

This section demonstrates the improvement achieved through the application of Lie theory via simulations. The proposed method is compared with the widely used quaternion-based EKF (Q-EKF) and the quaternion-based misalignment compensation EKF (QMC-EKF) [29]. While Q-EKF employs quaternion representation for attitude [30], it does not estimate or compensate for misalignment and does not utilize Lie theory. QMC-EKF estimates and compensates for the misalignment, and uses quaternions for attitude representation, However, it does not use Lie theory for the arithmetic operations.

The comparison focuses on two aspects: the accuracy of the state estimation results and the convergence property of internal variables. Estimation results refer to the estimated position, attitude, and misalignment. The internal variables represent measurement innovation and Kalman gain. Hereafter, the proposed approach is referred to as “Proposed” in the figures and tables, whereas “Q-EKF” and “QMC-EKF” denote the previous approaches, namely the quaternion based EKF and quaternion based misalignment compensation EKF, respectively.

4.1. Test under High Maneuverability Conditions

To clearly observe the improvements by the proposed method in the simulations, the underwater vehicle was assumed to perform highly dynamic three-dimensional maneuvers. Furthermore, to test the robustness of the proposed method, the noise, which is an unexpected perturbation, was added to the sensor data. The scenarios for the maneuver of the underwater vehicle and sensor noise are as follows:

• The initial position of the underwater vehicle is $(10,10,30)\left(\mathrm{m}\right)$, and the initial attitude is $(0,0,0)\left(\mathrm{rad}\right)$. The true misalignment of DVL from INS is (10°, −20°, 30°).
• The underwater vehicle maneuvers for 1200 s. The measurement rate of the INS and DVL is 10 Hz.
• The linear velocity of the underwater vehicle is $\mathbf{v}\left(t\right)={\left[\begin{array}{ccc}10& 5& 5\end{array}\right]}^T\left(\mathrm{m}/\mathrm{s}\right)$ in the vehicle coordinate frame. The angular velocity is as follows.

  $$\mathit{\omega }\left(t\right)=\left\{\begin{array}{cc}{\left[\begin{array}{ccc}-\frac{\pi }{5}& -\frac{\pi }{5}& -\frac{\pi }{5}\end{array}\right]}^T\left(\mathrm{rad}/\mathrm{s}\right),\hfill & 0\mathrm{s}\le t<600\mathrm{s}\hfill \\ {\left[\begin{array}{ccc}\frac{\pi }{5}& \frac{\pi }{5}& \frac{\pi }{5}\end{array}\right]}^T\left(\mathrm{rad}/\mathrm{s}\right),\hfill & 600\mathrm{s}\le t<1200\mathrm{s}\hfill \end{array}\right.$$

  (44)
• The measurement data from the simulated sensors contain random noise. The measurement noise in the linear velocity provided by the DVL is ${\mathbf{n}}_{\mathbf{v}}\sim \mathcal{N}\left(\mathbf{0},0.2{\mathbf{I}}_3\right)$. The measurement noise in the angular velocity provided by the INS is ${\mathbf{n}}_{\mathit{\omega }}\sim \mathcal{N}(\mathbf{0},0.01{\mathbf{I}}_3)$.
• Gaussian noise with the mean 0 and the standard deviation 0.7 m is applied to the measured position data in each coordinate, whereas Gaussian noise with the mean 0 and the standard deviation 0.03 rad is applied to the measured attitude in each coordinate.

Figure 2 explains the robustness of the position estimation result. It displays the estimation error and standard deviation ($\sqrt{\mathbf{P}\left(t_k\right)(1,1)}$) calculated by Equation (43) for the estimated x coordinate. The position estimation error remains within the standard deviation for the entire range. In addition, the standard deviation is kept within a bounded range, and does not diverge. At time $t=600\mathrm{s}$, the angular velocity of the vehicle changes abruptly. Figure 2 shows that there is a noticeable change in the standard deviation at $t=600\mathrm{s}$ as indicated by the two circles. Nevertheless, the standard deviation returns to its normal value instantly. This indicates that the proposed method is stable and robust in dynamic maneuvering conditions.

The error of the estimated position in the NED coordinate frame is compared to those of the Q-EKF and QMC-EKF, as shown in Figure 3. Here, the error of the estimated position is compared in terms of the distance error. The distance error refers to the distance between the estimated position $\widehat{\mathbf{p}}\left(t\right)={\left[\begin{array}{ccc}\widehat{x}\left(t\right)& \widehat{y}\left(t\right)& \widehat{z}\left(t\right)\end{array}\right]}^T$ and the reference position $\mathbf{p}\left(t\right)={\left[\begin{array}{ccc}x\left(t\right)& y\left(t\right)& z\left(t\right)\end{array}\right]}^T$ as defined by Equation (45).

$$d(\widehat{\mathbf{p}}\left(t\right),\mathbf{p}\left(t\right))=\sqrt{{(\widehat{x}\left(t\right)-x\left(t\right))}^2+{(\widehat{y}\left(t\right)-y\left(t\right))}^2+{(\widehat{z}\left(t\right)-z\left(t\right))}^2}$$

(45)

The proposed method exhibits smaller errors than the others, the entire time. This performance improvement was attributed to the accurate Jacobian matrix calculation, the use of proper plus and minus operations, and rigorous covariance calculation.

The position estimation performance is compared in

Table 1. Analysis of the position estimation error in the NED coordinate frame (m).

Stat	Di.		Position
		Q-EKF	QMC-EKF	Proposed
Mn	x	−0.9521	−0.0161	−0.0109
	y	−0.9562	−0.0183	−0.0117
	z	−1.0157	−0.0238	0.0007
Std	x	1.1995	0.4284	0.2266
	y	1.1828	0.4272	0.2258
	z	1.0719	0.4443	0.2322
RMS	x	1.5314	0.4287	0.2269
	y	1.5209	0.4276	0.2261
	z	1.4766	0.4450	0.2322

Stat: Statistic. Di.: Direction. Mn: Mean of the error. Std: Standard deviation of the error.

and

Table 2. Comparison of distance error under high maneuverability conditions (m).

Stat	Distance
	Q-EKF	QMC-EKF	Proposed
Mn	2.6026	0.6915	0.3617
Std	0.2558	0.2941	0.1602
RMS	2.6151	0.7514	0.3956

. Table 1 compares the position estimation errors in each coordinate, and Table 2 compares the distance error. Position errors in x, y, and z indicate $\left(\widehat{x}\left(t\right)-x\left(t\right)\right)$, $\left(\widehat{y}\left(t\right)-y\left(t\right)\right)$, and $\left(\widehat{z}\left(t\right)-z\left(t\right)\right)$, respectively. Based on the data presented in Table 1 and Table 2, the position and distance errors are compared visually in Figure 4 and Figure 5, respectively. The proposed method significantly reduces the errors in terms of the mean, standard deviation, and Root Mean Square (RMS) for all three axes. The RMS in x-directional position error is calculated as follows.

$$\sqrt{\frac{1}{N}\sum _{k=1}^N{\left(\widehat{x}\left(t_k\right)-x\left(t_k\right)\right)}^2},$$

(46)

where N indicates the number of estimations. The mean of the position error in x-direction without compensation of misalignment (Q-EKF) is −0.9521 m, and it is improved to −0.0109 m by the proposed method. The RMS of x-direction error is improved to 0.2269 m by the proposed method from 1.5314 m by the Q-EKF. The results confirm that the proposed method estimates the trajectories more accurately than the other approaches.

Figure 6 depicts the estimation of misalignments. The performance of the estimation for attitude and misalignment is shown in

Table 3. Analysis of the error in estimated attitude and misalignment (degrees).

Stat	Di.	Attitude			Misalignment
		Q-EKF	QMC-EKF	Proposed	Q-EKF	QMC-EKF	Proposed
Mn	$\varphi$	−0.2220	−0.0379	0.0018	NA	133.8953	−4.9228
	$\theta$	−1.3084	−0.0180	−0.0064	NA	−4.9177	0.1686
	$\psi$	1.6601	0.0062	−0.0030	NA	10.9918	−0.4202
Std	$\varphi$	1.7398	1.6499	0.4997	NA	87.1410	10.5798
	$\theta$	1.9603	1.2131	0.6366	NA	9.0491	0.7762
	$\psi$	2.0898	1.2046	0.6383	NA	11.8913	1.0958
RMS	$\varphi$	1.7539	1.6503	0.4997	NA	159.7545	11.6690
	$\theta$	2.3569	1.2132	0.6366	NA	10.2990	0.7643
	$\psi$	2.6689	1.2046	0.6383	NA	16.1933	1.1736

$\varphi$: Roll, $\theta$: Pitch, $\psi$: Yaw. NA: Not Applicable.

. The Q-EKF estimates the attitude of the underwater vehicle; however, it does not estimate the misalignment, and thus the statistics for Q-EKF corresponding to the misalignment are labeled as “Not Applicable (NA).” Figure 7 and Figure 8 present a graphical comparison of the errors in the estimation of attitude and misalignment based on Table 3. The proposed method exhibits improved estimation for both the attitude and misalignment. Unlike Q-EKF, QMC-EKF and the proposed method explicitly estimate the misalignment and compensate for the misalignment. However, QMC-EKF regards the attitude and the misalignment as the variables in a linear space. Therefore, QMC-EKF uses the usual arithmetic operations by which linear variables are treated. These operations result in approximations and require ad hoc normalizations. On the contrary, the proposed method uses Lie theory which provides rigorous arithmetic operations required to implement the Kalman filter. The improvements in estimation accuracy and stability conform to the theoretical verification [25] and are consistent with the examples shown in [26].

For QMC-EKF, the estimated misalignment has large errors, especially in the roll. If the vehicle moves in one of the principal axes, the misalignment in that direction does not affect the velocity measurement. Therefore, if the vehicle moves in the x-direction, the misalignment in roll is not distinctively perceivable by the velocity measurement. In the simulation, the x-directional velocity is twice the other directional velocities, and the estimation accuracy of roll misalignment is the worst of all. In addition to this effect, the approximations involved in QMC-EKF deteriorate the misalignment estimation in roll.

As shown in Figure 6 and Table 3, QMC-EKF exhibits unstable estimates in both pitch and yaw. On the contrary, the proposed method improves the estimation of misalignment and provides stable misalignment value. The proposed method estimates the misalignment in pitch as $-19.{8314}^{\circ }$, whereas QMC-EKF estimates it as $-24.{9177}^{\circ }$. As for the misalignment in yaw, it is estimated as $29.{5798}^{\circ }$ and $40.{9918}^{\circ }$, by the proposed method and QMC-EKF respectively. The proposed method exhibits improved estimation for both the attitude and misalignment. The proposed method not only reduces the mean error but also exhibits significant improvements in standard deviation and RMS errors.

The proposed method significantly improves the inherent properties of the estimation: Kalman gain and measurement innovation. The EKF based on the Lie theory, can model a nonlinear system more accurately. The linear approximation of arithmetic operations on rotation or attitude, which inherently exists in nonlinear space, necessitates ad hoc measures. As a remedy for this limitation, the use of Lie theory enables the rigorous application of the KF. Consequently, the convergence of the Kalman gain and measurement innovation as well as the covariance is improved.

Because the Lie theory provides accurate calculation of predicted measurement and the difference between two attitudes, measurement innovation becomes more accurate. Also, the covariance matrices represent the uncertainties more effectively. Therefore more appropriate calculation of Kalman gain is possible. This improves the convergence of the Kalman gain and measurement innovation.

Figure 9 compares the Kalman gain and measurement innovation of the methods. They are calculated by using Equations (39) and (38). The Kalman gain and measurement innovation for the proposed method is lower than the others. Moreover, they exhibit lower variance than the others. Overall, the simulation results indicate that the proposed method outperforms the others in the accuracy and stability of estimation.

4.2. Test under Low Maneuverability Conditions

To demonstrate that the improvement by the proposed method is more significant in high maneuverability conditions, the proposed method is tested under low maneuverability conditions. The ROV moves at a velocity of (1, 0.5, 0.5) m/s, indicating lower maneuverability than the velocity (10, 5, 5) m/s which is used in Section 4.1 for high maneuverability conditions. The other test conditions are the same as those for high maneuverability conditions.

Table 4. Comparison of distance error under low maneuverability conditions (m).

Stat	Distance
	Q-EKF	QMC-EKF	Proposed
Mn	0.6351	0.2964	0.2153
Std	0.2508	0.1301	0.0914
RMS	0.6828	0.3237	0.2339

and Figure 10 compare the trajectory estimation performance by the three methods. They analyze the distance error of the estimated trajectory. The proposed method results in the lowest distance error in average, standard deviation, and RMS, among the three. More importantly, when comparing Figure 5a,b with Figure 10b,c, the performance differences among the three methods are more remarkable under high maneuverability conditions than under low maneuverability conditions. The improvement by the proposed method is more significant when the vehicle moves dynamically.

Figure 11 illustrates the estimated results for the misalignment in the pitch and yaw. The proposed method provides a better estimation of misalignment than the QMC-EKF. Specifically, the misalignment in pitch is estimated as −19.0503° and −20.9881° by the proposed method and the QMC-EKF respectively. As for the misalignment in yaw, the estimations are 28.2626° and 19.0502° by the proposed method and the QMC-EKF respectively. Furthermore, the error by the proposed method has less standard deviation and RMS than the QMC-EKF. The RMS of the error in yaw misalignment estimation is 2.2311° and 11.4136° by the proposed method and QMC-EKF respectively. Overall, the proposed method exhibits improved estimation for the estimation of misalignment.

5. Experiments and Results

5.1. Test Conditions

The proposed method is tested using a light work-class remotely operated vehicle (ROV). There are two experiments in different environments. The first one is the experiment in a test tank and the second one is the experiment offshore in the East Sea, Republic of Korea. The first one will be called the tank test, and the second one will be called the field test. The detailed specifications of the ROV are listed in

Table 5. ROV specifications.

ROV Specificaitions
Dimension	(L) 2000 mm (W) 1300 mm (H) 1500 mm
Weight	1500 kg
Depth rating	2500 m
DOF	6 (Surge, Sway, Heave, Roll, Pitch, Yaw)
Speed	Forward: 2.5 knots, Lateral/Heave: 2 knots

. Figure 12 shows the ROV, sensors, test tank experiment, and the offshore experiment. The ROV is equipped with an INS, DVL, depth sensor, and USBL transceiver and is controlled remotely via human operators. The manufacturers and models of each sensor are listed in

Table 6. Sensor manufacturers, models, and specifications.

Sensor	Manufacturer and Model	Specifications
INS	ADVANCED NAVIGATION	Roll & Pitch Accuracy 0.01°
	Spatial FOG	Heading accuracy 0.01°
DVL	Teledyne RD Instruments	Velocity range 10 m/s
	Navigator Doppler Velocity Log	Velocity resolution 0.1 cm/s
Depth	Digiquartz technology, Submersible	$0.01\%$ full scale accuracy
	Depth Sensors Series 8000	$1\times {10}^{-8}$ Resolution
USBL	Sonardyne, RANGER 2 MF	Range accuracy $2.5\%$ slant range
	GYRO USBL 5000/WSM6+	Position repeatability $0.1\%$ slant range
	Type 8370	Relative position accuracy $5\mathrm{cm}$

.

In the tank test, the ROV was submerged to a depth of approximately 2.5 m in a water tank and traveled 140 m with a square path. In the field test, the ROV was submerged to the depth of approximately 134 m and operated to travel 200 m with a straight line path.

The position data from USBL is given relative to the USBL coordinate system not a fixed reference coordinate system. Position data provided by USBL is converted to absolute position data using the location and attitude data of the USBL. In the tank test, the USBL is fixed on the construction of the test pool, thus conversion is not required. In the offshore field test, the USBL transceiver is mounted on the ship, necessitating the conversion process. The conversion uses the attitude data from the INS embedded in the USBL system and location data from a DGPS on the ship.

5.2. Position Estimation Results

Position estimation results are compared and analyzed. The reference position trajectory is determined based on the USBL data. The position data from the USBL is first interpolated, then the interpolated data is smoothed, and finally, the smoothed data is used as the reference data. The USBL provides location data at a slower rate than the other data from DVL and IMU. Therefore, the position data are interpolated to be synchronized with the other measurement data. Then, to reduce the measurement noise, the interpolated data are smoothed using weighted mean filtering.

Figure 13 shows part of the position estimation result in the tank test and the field test. The line referenced by “Reference(filtered)” indicates the reference trajectory calculated by the method explained above. Figure 13 depicts five sets of data: raw sensor data, filtered reference data, and three trajectories estimated by Q-EKF, QMC-EKF, and the proposed method.

Table 7. Analysis of the distance error (m).

Stat	Tank Test			Field Test
	Q-EKF	QMC-EKF	Proposed	Q-EKF	QMC-EKF	Proposed
Mn	1.3553	0.2365	0.0863	0.9718	0.2606	0.0708
Std	0.7311	0.1507	0.0623	0.6637	0.1836	0.0694
RMS	1.5399	0.2804	0.1065	1.1769	0.3187	0.0991

compares the distance errors of the estimated trajectories. Figure 14 and Figure 15 depict the analysis shown in Table 7. These results align with the simulation test results, specifically those obtained under conditions of low maneuverability.

5.3. Misalignment Estimation Results

Figure 16 depicts the estimated results for the misalignment of the pitch in the tank test and yaw in the field test. Two plots are presented because only two of the methods estimate the misalignment. Among the methods considered, only QMC-EKF and the proposed method estimate the misalignment, resulting in two available plots. As indicated by ’NA’ in Table 3, Q-EKF does not estimate the misalignment.

As shown in Figure 16a, QMC-EKF exhibits unstable estimates between 46–87 s, during which the ROV started maneuvering. Also, as shown in Figure 16b, QMC-EKF exhibits unstable estimates between 614–701 s, during which the ROV changes its operation mode from dive to horizontal maneuvering. On the contrary, the proposed method improves the estimation of misalignment and provides stable misalignment value. The proposed method estimates the misalignment in pitch as −0.166°, whereas QMC-EKF estimates it as 0.194° which is read after it is stabilized. As for the misalignment in yaw, it is estimated as −1.707° and −2.625° by the proposed method and QMC-EKF respectively. As with the pitch misalignment, it is read after it is stabilized in the case of QMC-EKF. Because the rotation between the INS and DVL does not change during the operation, the misalignment is expected to be kept constant during the operation. The misalignment estimated by the proposed method is kept constant as expected. However, the misalignment estimated by QMC-EKF exhibits an unstable change, attributed to the approximations and adjustments made against the nonlinear nature of the misalignment.

5.4. Improvements in Internal Properties

The improvement in internal properties is also demonstrated clearly in the experiments. Figure 17 and Figure 18 show the covariance, Kalman gain, and measurement innovation of the estimated position in the tank test and field test, respectively. Covariance, Kalman gain, and measurement innovation are internal parameters specific to the three methods: Q-EKF, QMC-EKF, and the proposed method. These parameters do not apply to the raw sensor data or the filtered reference data, which are included in Figure 13. Consequently, Figure 17 and Figure 18 have only three plots.

In both experiments, the covariance of the estimated x-directional coordinate is the smallest for the proposed method. Also, it is kept steady all the time. The smaller value of covariance indicates that the estimated variable has lower uncertainty. Steady covariance indicates that the estimation is robust to disturbances. Figure 18d shows that the proposed method has superior convergence characteristics, both during the ROV’s diving phase (0–670 s) and its subsequent horizontal motion (after 670 s). As a whole, the results of the experiments show that the stability of the proposed method is superior to the others, while the estimation accuracy is at least comparable to the others.

6. Conclusions

This paper presented a Lie theory approach for the navigation of unmanned underwater vehicles that compensates for the misalignment between the INS and DVL. Specifically, we employed the Lie theory for the implementation of the KF and formulated the process and measurement models in Lie groups. The pose and misalignment increments were evaluated and corrected using Lie algebra and exponential operations, and the covariances of the pose and misalignment were appropriately represented in Lie algebra. Consequently, the proposed method enhances the estimation performance in both the estimation accuracy and internal properties. Specifically, the use of Lie theory brings about improved convergence of the covariance, Kalman gain, and measurement innovation. The improvements were validated through simulation and field experiments.

In future research, we plan to apply Lie theory to tracking surface vessels. Applying Lie theory to the tracking of surface vessels is crucial, particularly when precise tracking is required during high-intensity maneuvers. This is because, in the absence of Lie theory, the highly dynamic process amplifies approximation errors, leading to the breakdown of the estimation procedure’s stability. It is expected that the use of Lie theory will provide remarkable improvement in this case.