Invariant Error-Based Integrated Solution for SINS/DVL in Earth Frame: Extension and Comparison

The error propagation of traditional strapdown inertial navigation system (SINS)/Doppler velocity log (DVL) is not autonomous because its error state model is trajectory-dependence. Recently, the invariant error defined on the Lie group has raised much attention due to its trajectory-independent and autonomous error propagation. In this article, the invariant error-based Kalman filter for the SINS/DVL integration solution is investigated with a main focus on its extension and comparison. The contributions of this study are threefold. First, the invariant error defined on the matrix Lie group (group of double direct isometrics) is extended to model the nongroup affine traditional SINS mechanism and the group affine transformed SINS mechanism; both of them are derived in Earth frame and augmented with the drift and bias of the inertial measurement units (IMUs). Then, the observation equations for different invariant error-based state models are derived for SINS/DVL application, a theoretical analysis is performed, and a comprehensive evaluation is conducted under different maneuvering conditions by lake field trial. Finally, the variational Bayesian approach is introduced into the invariant error-based Kalman filter to infer the inaccurate process noise covariance matrix (PNCM) and time-varying measurement noise covariance matrix (MNCM) from a practical perspective; experimental results demonstrate that it can improve the navigation accuracy significantly. This study is expected to facilitate the selection of appropriate invariant error to SINS/DVL applications.

Left-and right-invariant Lie algebra error state including sensor bias under traditional group affine SINS mechanization in Earth frame, respectively. T l , T r , T lg , T rg Transformation matrix between invariant error state covariance and traditional error state covariance.

(·), (·)
Priori estimate vector of state and posteriori estimate vector of state.

I. INTRODUCTION
A UTONOMOUS underwater vehicles (AUVs) play a critical role in biological monitoring, oceanographic surveys, military underwater surveillance, and so on, and all of these tasks are dependent on the accurate navigation information provided by AUVs [1], [2], [3]. Since the electromagnetic-based signals with position functional will attenuate rapidly underwater [3], [4], underwater localization is a particularly challenging problem compared with landbased vehicles' location. Fortunately, the strapdown inertial navigation system (SINS) integrated with other complementary acoustic navigation techniques, such as Doppler velocity log (DVL) [6], [7], long baseline (LBL)/short baseline (SBL)/ultrashort baseline (USBL), and single beacon [4], [5], can be used for underwater accurate positioning. The LBL/SBL/USBL and single beacon generally require extensive This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ setup and expensive investment and are easily disturbed by cluttered environment, while DVL is an alternative for its flexibility and autonomy [8]. The SINS/DVL integrated navigation system can alleviate the inevitably accumulated errors and becomes the commonest navigation solution for underwater vehicles [9], [10].
For integrated navigation applications, there are two different integration procedures, direct model [12] and indirect model [13], [14], which can be used for SINS/DVL. The navigation states, including attitude, velocity, and position, will be selected directly as state variables for the direct model. The latter, established on error state-space model, is more preferred as the superiority of linear Kalman filter in terms of computational efficiency [14]. Many Kalman filter methods can be used for error estimation [11], [15]. Traditionally, the extended Kalman filter (EKF) is usually used for SINS/DVL integrated navigation. The EKF usually approximates the integrated navigation system, which is a nonlinear system in different navigation frames, by first-order approximation to stabilize the linearized estimation error [15]. The traditional EKF adopts this kind of linearization operation attempt to derive local convergence properties around any trajectory. However, the local stability of EKF is aleatory [17], [18], [19] and may suffer from inconsistency in some applications, such as simultaneous location and mapping (SLAM) [16]. It follows the assumption that the estimated error is sufficiently small to be propagated analytically through the first-order linearization around the previously estimated trajectory. When the estimated trajectory is far from the truth, the linearization based on the estimated trajectory is uncredible, and it may induce positive feedback to the filter [19].
In fact, the reason for this phenomenon can be attributed to its error definition. Different error state definitions can lead to different performances. Hartley et al. [20] derived a contactaided inertial navigation observer for a 3-D bipedal robot using the theory of invariant observer design, and the contactaided InEKF provides better performance in comparison with the quaternion-based EKF. Li and Chang [21] revisited the MEKF for INS/GPS with the utilization of navigation frame attitude error model rather than the attitude error that is used as the local filtering state expressed in the body frame. Heo and Park [22] defined the error in the Lie group to address the inconsistency of EKF. Wang et al. [23], [24] transformed the traditional velocity error representation with a new velocity error considering the attitude error and proposed a state-transformed EKF (STEKF) to improve the consistency and accuracy of integrated navigation. The traditional state error is commonly defined as the algebraic difference of vectors without considering the vector's representation [25], [26]. The invariant EKF (IEKF) [15], [19], whose error state defined by invariant error, derived based on the matrix Lie Group has proven that it outperforms the EKF in theory and experiment. Actually, the error state model, where the attitude error is usually defined in a special orthogonal (SO) group, while another error is defined in Euclidean space, has been widely used in inertial applications, especially in attitude estimation [17], [21].
Recently, the group of double direct spatial isometries SE 2 (3) is introduced to formulate the attitude, velocity, and position into a group [15]. Through formulating the navigation parameter into SE 2 (3), the vector error state corresponding to the SE 2 (3) group-based invariant error satisfies a log-linear autonomous differential equation if the group-based state dynamical system is group affine [15]. However, the primitive SINS dynamic equation embedded on group is not completely group affine and can be regarded as an approximated model if ignored the Coriolis and centrifugal force. In the Earthcentered Earth-fixed (ECEF) frame, the SINS mechanism can be transformed into a group affine form [27], [28], [29], and the striking group affine property, the ability to handle the large initial attitude misalignments in inertial alignment, can be fully exploited [30], [31], [32]. The invariant error state on the matrix Lie group, named left-or right-invariant error, is defined as the product between the inverse of the true state and the estimated state or conversely. Introducing an auxiliary in Earth frame or adopting the GNSS-aided decomposition procedure in the east-north-upward (ENU) frame can make the SINS mechanism satisfies group affine and further embed into the IEKF framework [31]. From the common frame error definition perspective, state transformation techniques have been applied to the coordinate-frame consistency of velocity error vector [23], [24] but not take the position error into consideration. Regardless of whether the SINS differential equation embedded on the group satisfies the group affine, the invariant error can be employed to construct the error state model, just as the traditional error definition can be utilized to model the error differential equation in different navigation frames. The position error expressed in the ENU frame is usually tiny, which will cause numerical instability in the Kalman filter calculation. This article only investigated the error state in the Earth frame.
For SINS/DVL integrated applications, there remain some issues that need to be addressed. On the one hand, the measurement accuracy of DVL will not exceed 0.3% under a low dynamic environment, while it can be up to 2% in a complex environment, and the measurement noise is unknown and time-varying [7]. Thus, it is unrealistic to set proper parameters for all conditions and it may lead to parameter mismatch. On the other hand, even though the attitude, velocity, and position can be embedded on the Lie group, the drift and bias of inertial measurement unit (IMU) cannot, which play an important role in navigation and cannot be acquired accurately before navigation. However, the Kalman filter works well based on the assumption that the accurate process noise covariance matrix (PNCM) and the measurement noise covariance matrix (MNCM) can be acquired in advance [33]. There is lack of solutions for dealing with the adaptivity of invariant error-based Kalman filter. To address the parameter mismatch, adaptive methods, including correction, covariance matching, maximum likelihood (ML), and Bayesian inference, can be used for the Kalman filter [33], [34], [35], [36], [37], [38], [39], [40]. Wang et al. [36] introduced the improved Sage-Husa adaptive Kalman filter (SHAKF) into an SINS/DVL tightly integrated model, where four beams' measurements are used rather the 3-D velocity. Based on the ML principle, a new adaptive unscented Kalman filter (UKF) with process noise covariance estimation is proposed to enhance the UKF robustness against process noise uncertainty [37]. In [38], to improve the speed of convergence, rotation vectors instead of traditional Euler angles were used and developed an algorithm to automatically tune the MNCM using adaptive Kalman filtering. In [39], an algorithm based on variational Bayesian (VB) for attitude estimation was proposed. A robust initial alignment method for SINS/DVL is proposed in [40] to solve a practical applicable issue, which is that the outputs of DVL are often corrupted by the outliers. The mentioned above adaptive filters almost cannot estimate the process noise and measurement noise simultaneously. However, a novel VB-based adaptive Kalman filter (VBAKF) was proposed in [33] to estimate an inaccurate and slowly varying MNCM and unknown PNCM. The simulation results demonstrated that the VBAKF outperforms the existing state-of-the-art filters, and it is a good choice to employ the VB into SINS/DVL application to enhance its adaptive. Wang et al. [34] and Wang and Chen [35] employed the VB approach into SO(3)-based IEKF for attitude estimation for spacecraft and robotic applications where the inaccurate process noise or heavy-tailed process noise occurred. In our following method, we employed the VB into SE 2 (3) IEKF, the Lie group adopted is not the same as in [34] and [35], to infer the inaccurate process noise and the time-varying measurement noise at the same time for SINS/DVL integrated system, and verified its adaptive by field test.
As mentioned above, this article focuses on the extension of invariant error-based Kalman filter for SINS/DVL and its adaptivity. The contributions of this article include the following.
1) The derivation of error state models considering IMU bias based on the SE 2 (3) left-and right-invariant error definition for SINS mechanization and transformed SINS mechanization in the Earth frame.
2) The theoretical analysis points out that the left-invariant error definition is more appropriate for SINS/DVL integrated navigation when the IMU bias is considered regardless of the error state dynamic that is group affine or not. The theoretical analysis is well verified by the lake field test on different maneuvering conditions.
3) The VB approach is introduced into invariant errorbased Kalman filter to infer the PNCM and MNCM online, and experiments demonstrated that it can achieve superior performance for its adaptive capabilities.
The remainder of this article is organized as follows. Section II gives a brief SINS mechanization in the Earth frame and mathematical preliminaries about the Lie group, invariant error representation, and group affine. The error state models based on invariant error under nongroup affine and group affine are derived in Section III, and the model analysis is also performed. The observation equations for different error models are deduced in Section IV, and VB is adopted to infer the time-varying MNCM and inaccurate PNCM. In Section V, a lake trial experiment is conducted to make a comparison and evaluation. Conclusions and suggestions are drawn in Section VI.

II. SINS MECHANIZATION AND MATHEMATICAL PRELIMINARY
In this section, SINS mechanization and transformed SINS mechanization in ECEF are presented. Then, some useful matrix Lie group and Lie algebra and its relationship are presented. The transformed SINS mechanization dynamics with Lie group representation that is group affine is verified.

A. SINS Mechanization in ECEF
The traditional SINS mechanization in ECEF is given by [30] ⎧ ⎪ ⎨ where e denotes the ECEF frame, i denotes the inertial frame, and b denotes the body frame. (·×) denotes the skewsymmetric matrix operation. C e b is the direction cosine matrices from b-frame to e-frame. v e and p e are the velocity and position in the e-frame, respectively. ω b ib is the body angular rate expressed in the b-frame measured by the gyroscopes. f b is the specific force in the b-frame measured by the accelerometers. ω e ie is the Earth rotation in e-frame. g e is the gravity vector, and the relationship with the gravitational vectorḡ e is given by The transformed SINS mechanization is formulated by introducing an auxiliary velocity vector [25] ⎧ ⎪ ⎨ where the auxiliary velocity vector is given by Remark 1: The transformed SINS mechanization is derived in the ECEF frame while not in ENU frame due to the strong coupling of navigation states. The transformed SINS mechanization in ECEF is group affine, while SINS mechanization in ENU frame is not; this property leads to two dominantly striking advantages that the error is trajectory-independent and log linearity [15]. The detailed explanation will be given in Section II-C.

B. Matrix Lie Group and Lie Algebra
The Lie group encompassed the concepts of group and smooth manifold in a unique body means that a Lie group is a smooth manifold whose elements satisfy the group axioms [41], and the calculus can be conducted to the unique tangent space of each point; the tangent space is a linear or vector space. In the general navigation and position application, the matrix Lie group SE 2 (3), also called the group of double direct isometrics [16], [19], is used to represent the extended pose as where χ is a 5×5 matrix with attitude rotation matrix C ∈ SO (3) which is a special orthogonal group, the velocity vector v ∈ R 3 and position vector p ∈ R 3 . The inverse of group state is given by The Lie algebra associated with SE 2 (3) is given by The Lie algebra SE 2 (3) space is equivalent with the 9-D Euclidean space in representation as the operation (·^) is a linear map operation. The Lie algebra and the Lie group can be transformed by each other by exponential mapping exp(·) and logarithmic mapping log(·) as follows: where ϕ = ζ a is the rotation vector, ζ denotes the rotation angle, and a is the unit-length axis of rotation. exp m (ϕ×) is the matrix exponential operation, exp m (ϕ×) ≈ I 3 + (ϕ×) if ζ is small angle. J l and J −1 l are given by when ζ is assumed to be small, and then, J l ≈ I 3 +(ϕ×)/2 and

C. Group Affine and Invariant Error Representation
Consider that a class of dynamical systems with differential equationχ = f u (χ ), where χ lives in the Lie group and u is an input variable. Define two different nonlinear errors, namely, the left-invariant error η l =χ −1 χ and right-invariant error η r = χχ −1 , where χ is the ground-truth value andχ , which lives in the Lie group, is the estimated value of χ .
Formulating the attitude C e b , velocityv e , and position p e as elements of the group SE 2 (3) The left-invariant error and the right-invariant error between χ andχ can be written as Then, (3) can be reformulated as the following compact form:χ where χ andχ are the solution of (15), the deterministic dynamics (15) satisfies the following equation: According to [15, Th. 1], a dynamic system, which follows the abovementioned relationship, is regarded as group affine dynamic, the left-invariant error and the right-invariant error are trajectory-independent, and the error model is independent of the global estimated states. Taking the left-invariant error as an example, it can be easily verified that the following equation always holds on for any χ and η l In the particular case where χ = I 5 , (17) becomeṡ For the right-invariant error, the following differential equation for η r can be obtained: Remark 2: In (18), it is shown that the differentiation of left-invariant error is undoubtedly trajectory-independent. However, the differentiation of right-invariant error is not in the strict sense, because U embeds the p e ib into g e . The p e ib is coupled into U by a second-order fractional quantity of ω e ie , and the error introduced is only (ω e ie ×) 2 δp e , which plays a limited negative effect, or even negligible. Therefore, the rightinvariant error differential equation can be regarded as trajectory-independent to some extent. It is easy to verify that the SINS mechanization in Eq. (1) cannot be rewritten into a compact form like Eq. (15) that W and U is independent of global states, whose differential equation for the invariant error is not group affine.
Consider the left-invariant error as define in Eq. (13), let A u be defined by g u (η l ) = g u (ξ l ) = (A u ξ l ) ∧ + O(ξ l 2 ) [15], the linear differential equation in R 9 can be write asξ l = A u ξ l . According to the log-linear property of error in [15], for arbitrary large initial errors ξ l,0 ∈ R 9 , if η l,0 = exp(ξ l,0 ). Then the nonlinear error for all times t ≥ 0 can be exactly recovered by η l,t = exp(ξ l,t ) without approximation error, which leads to follow the standard Kalman Filter guarantees especially with local asymptotic stability.

III. ERROR STATE MODEL AND ANALYZATION
In this section, the traditional error state model whose errors are defined as the direct differences of the vectors without considering the vector representation is first derived. Then, the error state model based on the left-and right-invariant errors is derived under the SINS kinematic without group affine. The error state model is derived based on the group affine transformed SINS mechanization. Finally, a heuristic analysis for different error state models is performed.

A. Traditional Error State Model
For the intermediate-grade and the navigation-grade IMU, the sensor errors of accelerometers and gyroscopes can be modeled as a random walk process where ε b is the gyroscope bias, w b g is the corresponding zero-mean Gaussian noise, ∇ b is the accelerometer bias, and w b a is the corresponding zero-mean Gaussian noise. Denote the attitude error corresponding to C e bC eT b in the Euler angle form as ϕ r . The velocity and position error are defined directly by the difference of the estimated value and the truth value. If ϕ r is assumed to be a small value According to (1) and (22), the traditional error state model is given by ib is the body angular rate with noise measured by gyroscopes, the special force with noise measured by accelerometers. More derivation detail can refer Appendix A. Actually, the traditional error can be viewed as definition on SO(3) + R 6 .
Furthermore, taking the bias of inertial sensors into consideration, define the traditional error state vector as The corresponding error state model (named SO) is given by a ] T and F and G can be referred in Table VII.

B. Invariant Error State Model Without Group Affine
For dynamic systems, whether the group affine is satisfied is not a necessary prerequisite for employing the SE 2 (3)-based invariant error representation. In other words, the SE 2 (3)-based invariant error is just an error representation, which can be employed for any deterministic system. Therefore, SE 2 (3)-based invariant error definition for dynamic systems (1) and its error propagation equation can be derived.
For the left-invariant error definition, denote the attitude error corresponding toC eT b C e b in Euclidean space as ϕ l , and denote the vector-form velocity and position errors corre- as dv e l and dp e l , respectively. If ϕ l is assumed to be a small value, the left-invariant error can be approximated as Define the error state vector as The derivation of left-invariant error state model augmented IMU bias for traditional SINS mechanization is given by is the Earth rotation is expressed in the b-frame. The detail of derivation above formulas can refer to Appendix B. Then, the corresponding error state model (denoted as LSE) is given by where F l and G l can be referred in Table VII. For the right-invariant error (14), denote the attitude error corresponding to C e  (14) as dv e r and dp e r , respectively. If ϕ r is assumed to be a small value, the rightinvariant error can be approximated as Define the error state vector as The derivation of right-invariant error state model augmented with IMU bias for traditional SINS mechanization is given by The detail of derivation can refer to Appendix C. Then, the corresponding error state model (named RSE) is given by where F r and G r can refer to Table VII.

C. Invariant Error State Model Within Group Affine
By introducing the auxiliary velocity vector (4) into traditional SINS mechanization (1), the transformed SINS mechanization (3) can be derived. The transformed SINS mechanization formulated within Lie group satisfies the group affine property and its error dynamics is exactly log linear. In fact, the log-linear error system is only an approximation of the existing sensor noise and IMU bias, and however, the performance on initial alignment is proven outstanding [30].
For the transformed SINS mechanization, the traditional velocity and position error and its differentiation are given by For the left-invariant error (13) within group affine system (15), denote the attitude error as ϕ l , and denote the velocity and position vector-form error corresponding to the element as dv e lg and dp e lg , respectively. The subscript means that the error is defined for group affine systems. If ϕ l is assumed to be a small value, the left-invariant error can be approximated as Define the error state vector as The derivation of left-invariant error state model augmented IMU bias for transformed SINS mechanization is given by The above derivation is similar to (28), but it can clearly conclude that the error dynamic is independent to estimated global states, while (28) is not. Then, the corresponding error state model (named LSEGA) can be rewritten as where F lg and G lg can be referred in Table VII. For the right-invariant error (14) within group affine system (15), denote the attitude error as ϕ r , and denote the velocity and position vector-form error corresponding to the element v e −C e bC e T bṽ e andC e T b (p e −p e ) as dv e rg and dp e rg , respectively. If ϕ r is assumed to be a small value, the right-invariant error can be approximated as Define the error state vector as The derivation of right-invariant error state augmented IMU bias for transformed SINS mechanization is given by The detail of derivation above formulas can refer to Appendix D. Then, the corresponding error state model (named RSEGA) is given by where F rg and G rg can be referred in Table VII.

D. Analyzation for Different Error State Model
The error state models in (25), (29), (33), (38), and (42) are denoted as SO, LSE, RSE, LSEGA, and RSEGA, respectively. For clarity, a summary table is provided in Table VII for different error state models. If the IMU measurement is noise-free and bias-free, LSEGA (RSEGA) will be independent with estimated states and its errors will be propagated autonomously. However, the IMU drift bias needs to be estimated to attenuate error divergence in practice, and the error state model will be slightly or strongly coupled by the estimated states. The relationship between the different error state models and the estimated states considering the IMU drift bias is summarized in Table I. It can be observed that RSE and RSEGA are more state-dependently than the other models, while LSEGA is completely (to some extent) independent with estimated states and its error propagation will be autonomous.
The LSE is not completely decoupled from the estimated states, the product of estimated attitude and the Earth rotation rate ω e ie ≈ 7.292115 × 10 −5 rad/s, andω b ie is coupled to the error model. However, this negative effect is limited compared to the product of attitude and velocity or position, that is to say, LSE will be better than the RSE (RSEGA).
Remark 3: Despite the fact that both the SO and LSE are associated with the estimated attitude, the mechanisms affecting their error propagation are different. For the SO, the estimated attitude matrix will be applied on both IMU measurements and bias, and the residual error will be propagated to the following error estimation through two channels, especially the attitude and velocity errors. However, for the LSE model, the residual error in attitude estimation will only act on ω e ie negligibly, and the experimental results in Section V can prove this theoretical inference. For the RSE and RSEGA models, it can be noted that the latter has less correlation with the estimated states, which only acts on the IMU bias due to the introduction of the group affine error representation, while the RSE has a more complex dependence on the estimated states.

IV. APPLICATION FOR SINS/DVL WITH VB
In this section, the measurement equations of SINS/DVL for different error models are first deduced. Then, the Kalman filter parameters (initial covariance) are initialized and feedback corrections are given for the different error state models. Finally, the VB approach is incorporated for invariant error-based state models to infer inaccurate PNCM and varying MNCM online, and an example algorithm is given for illustration.

A. Measurement Equations Establishment for SINS/DVL Integrated Navigation in Different Error State Model
For the integrated SINS/DVL, the measurement is the body velocity v b . It can be verified that the measurement is neither left-nor right-invariant observation for the transformed group state (13), but right-invariant for nontransformed group [30].
For the traditional error state model (25), the indirect error observation is derived as where H = [ −(v e ×) I 3 0 3×9 ] is the observation matrix for traditional error state mode (25), and the above derivation is based on the approximation (22). For the left-invariant error state model (29), the indirect error observation is derived as where H e r = [ 0 3 −I 3 0 3×9 ] is the observation matrix for the right-invariant error state mode (33).
For the left-invariant error state model (38) with transformed SINS mechanism, its indirect error observation is derived as where ] is the observation matrix for left-invariant error state mode (38).
Similarly, for the right-invariant error state model (42) with transformed SINS mechanism, its indirect error observation is derived as where H e rg = [ −(p e ×)(ω e ie ×) −I 3 (ω e ie ×) 0 3×6 ] is the observation matrix for the right-invariant error state mode (42).
Remark 4: It is important to realize that some observation matrices involved true state, such as H . However, the true state is unavailable in practice, and it can be compromised with the estimated states. This substitution will yield an approximation error, but this error can be treated as observation noise or an approximation of measurement noise. In Section IV-B, to mitigate the inaccuracy of the measurement noise and the error caused by the observation approximation, the VB approach will be introduced to infer the MNCM online.

B. Parameters Initialization and Feedback Correction
The initial covariance matrix will directly affect the filtering performance. However, it is difficult to acquire an accurate initial covariance matrix in practice. Generally, an initial diagonal covariance P 0 can be set for δx according to prior knowledge based on error interindependent assumption. However, for the error state model based on the invariant error definition (26), (30), (35), and (39), the errors are mutually coupled, so their initial covariances cannot be set directly and need to be translated based on the traditional method.
According to the left-invariant error definition in (26), the initial covariance for [ ϕ l dv e l dp e l ] T is given by For the right-invariant error definition in (30), the initial covariance for [ ϕ r dv e r dp e r ] T is given by ⎦ v e 0 andp e 0 are the initial velocity and position, respectively. According to the left-invariant error definition in (35), the initial covariance for [ ϕ l dv e lg dp e lg ] T is given by For the right-invariant error definition in (39), the initial covariance for [ ϕ r dv e rg dp e rg ] T is given by ⎦ v e 0 andp e 0 , are the initial auxiliary velocity and position, respectively.
The Kalman filter is employed to estimate and update the error states, as defined in (24), (27), (31), (36), and (40). In addition, the corrected error states are feedback to the SINS solution (navigation parameters) to inhibit its error divergence, i.e., error feedback correction. After the SINS states correction, the corresponding error states will be resettled to zero.
For the traditional error definition in (24), the feedback correction for navigation parameters at instant k is given by where (·) and (·) represent the estimated (priori) state and the corrected (posteriori) state, respectively. For the left-invariant error definition based on the nontransformed SINS mechanism, the navigation parameters will be corrected by p e k =p e k +C e b,k dp e l,k .
Similarly, for the right-invariant error definition, the feedback correction is given by For the left-and right-invariant error definitions based on the transformed SINS mechanism, the corresponding feedback correction is given, respectively, as follows: Remark 5: Since the estimated errors have been already fed into the global navigation parameters (attitude, velocity, and position), its corresponding error states need to be resettled, that is, δx(1:9) = 0 or dx l/r/lg/rg,k (1:9) = 0. It should be noted that the velocity and position corrections are perceived as a linear approximation to the exponential map operation of SE 2 (3), and this approximation does not lose accuracy because it is only used to construct nonlinear group errors [30]. According to the log-linear property of the group affine system, the leftand right-invariant errors can be recovered from the linear vector errors of the transformed SINS mechanism.

C. Uncertainty Processing With VB
The actual DVL measurement noise does not completely coincide with the desirable Gaussian distribution, as shown in Fig. 1. The measurement noise is time-varying as the vehicle's own mobility and environmental variability. The probability density functions (pdfs) of the east and north velocity noise are more difficult to be fit with a Gaussian than the upward velocity noise. In underwater navigation, the upward channel can be measured accurately using a depth meter, while the horizontal channel cannot. Therefore, adaptive measurement noise of velocity is required to enhance the navigation accuracy. System noise plays an essential role in integrated navigation, and it should also be adjusted to ensure good performance accuracy as much as possible [40].
To address the uncertainty of inaccurate PNCM and timevarying MNCM, the state together dx with the predicted error covariance matrix (PECM) P and MNCM R is inferred based on the VB approach by choosing the inverse Wishart priors for the PECM and MNCM [33]. Both the one-step predicted posterior pdf and the measurement likelihood pdf submit the Gaussian distribution p dx | z 1:k−1 ,P = N dx; dx,P (57) p(z k | dx, R) = N(z k ; Hdx, R) (58) where N(·;μ, ) denotes the Gaussian pdf, dx andP are, respectively, the one-step predicted error state vector and corresponding PECM, and H and R are, respectively, the discrete observation matrix and MNCM. Remark 6: In [33], inspired by the heuristic idea that the estimate of the state dx is more directly influenced by PECM, which may be more estimable than system noise matrix Q, the author prefers to infer the PECMP rather than infer Q directly. In this article, this heuristic idea is also adopted.
In Bayesian statistics, the inverse Wishart distribution is a good choice for characterizing the conjugate prior to the covariance matrix of Gaussian distribution with known mean. In this article, since bothP and R are the covariance matrices of Gaussian pdfs, the inverse Wishart pdf is chosen as the prior distribution for p(P | z 1:k−1 ) and p(R | z 1:k−1 ) where IW(·; μ, ) denotes the inverse Wishart pdf with degrees of freedom (DOFs) parameter μ and inverse scale matrix of inverse Wishart pdf. For the distribution of (59), t k|k−1 = n + τ + 1 andT k|k−1 = τP, where τ ≥ 0 is the tuning parameter and n is the dimension of system. For (60), where m is the dimension of measurement and ρ is the forgetting factor to adapt the time-varying measurement noise. The joint poster pdf p(dx,P, R|z 1:k ) will be used to infer the states along with PECM and MNCM together. However, the joint poster pdf does not have an analytic solution. The VB method can obtain an approximate solution by finding a free-form derived approximate pdf [33] p dx,P, R | z 1:k ≈ q(dx)q P q(R) where q(·) is the approximate posterior pdf of p(·) and given by minimizing the Kullback-Leibler divergence between the factored approximate posterior pdf and the true joint poster pdf. The optimal solution satisfies the following equation where {dx,P, R} is the jointed to-be-estimated parameter, κ is an arbitrary element of , (−κ) is the set that does not involve κ, the c κ is a constant related only κ, E[·] and log(·) denotes the expectation operation and logarithmic function respectively. In general, the local optimum of (62) can be obtained by employing fixed-point iterations.
According to the Gauss-inverse-Wishart conditional independence property, the joint pdf can be factored as The variational measurement update contains three-step as follows. First, update the q(P) given q(dx) by q P = IW P ;t k ,T k = IW P ;t k|k−1 + 1, [33]. Let dx = dx when the first iteration. Then, update the q(R) given q(dx) by Finally, update the q(dx) based on the inferred q(P) and q(R) by where dx = dx + K(z k − Hdx) is the corrected state by Kalman filter,P =P − KHP, K =PH T (HPH T + R) −1 is the Kalman filter gain. After fixed-point iteration N as (64)-(66), the variational approximations of posterior pdfs can be obtained. More details refer to [33].
In order to illustrate unambiguously how to incorporate the VB with invariant error-based models for adaptive inference of PNCM and MNCM. Here, only give the VB-LSEGAKF implementation pseudocode incorporating LSEGA, and the variational measurement update is shown in Algorithm 1.
Remark 7: For our SINS/DVL application in this article, the tuning parameter τ and the forgetting factor ρ are chosen by default to 2 and 0.98, respectively. Under the assumption that the measurement noise is independent, the DOF parameter u 0 = 9 is set by default. The initial MNCM R is assumed to have an inverse Wishart pdf andÛ 0 = (û 0 − m − 1)R according to [33]. Actually, when ρ = 1,û 0 = 1, and τ = 1, the VB-LSEGAKF will be boiled down to LSEGAKF. The iteration number N = 5 with considering the navigation accuracy and computational efficiency. Unfortunately, the navigation accuracy does not increase infinitely with the iterations. It should be noted that this parameter setting is appropriate for our SINS/DVL and it may not perform well for other integrated systems.
V. EXPERIMENTS STUDY A boat-mounted lake field trial was conducted at Lake Mulan in January 2022 to evaluate the proposed algorithm. The equipment used in the lake trial and its trajectory are shown in Fig. 2(a) and (b), respectively. The experimental platform contains three main parts, i.e., high-precision SINS, GPS, and DVL. The SINS is equipped with laser gyroscopes and quartz accelerometers; the SINS system can collect the IMU measurement and it can also provide high-precision reference information when GPS is available. The GPS module contains an OEM628 series receiver and antennas; three GPS systems are adopted in this test: one is provided for highprecision SINS and the other two work in PPP and RTK modes to provide positioning results. The DVL (PA600), developed Algorithm 1 The VB-LSEGAKF for SINS/DVL Initialization: m = 3, n = 15, N = 5, ρ = 0.98, τ = 2,û 0 = 9, t = 0.005, dx e lg,0 ← 0,    by the Institute of Acoustics, Chinese Academy of Sciences, was mounted on boat in this field trial. The major sensor parameters are summarized in Table II.  The lake field experiment lasts for 12 000 s. The first 1000 s are devoted to the initial alignment and calibration process, which is not the focus of this article. Their reference attitude and velocity are shown in Figs. 3 and 4, respectively. In addition to the entire test data for algorithm validation (named Case 1), a sequence of smooth maneuvering condition (red trajectory in Fig. 2(b), which goes from 4000 to 5500 s in Figs. 3 and 4, denoted as Case 2) and a sequence of intense maneuvering condition (green trajectory in Fig. 2(b), which goes from 8000 to 9500 s in Figs. 3 and 4, denoted as Case 3) were selected to verify the effectiveness of the proposed algorithms for different maneuvering conditions. The mean absolute error (MAE) and the root-mean-square error (RMSE) are used to evaluate the performance of algorithm. The MAE and RMSE are defined as follows: Besides this, the following relative metric horizontal position error (Hori-Error) percentage will be adopted [24]: where D is the distance traveled.

Remark 8:
This article focuses on the horizontal error since the vertical channel value can be accurately measured by depth meter for underwater navigation. In the following comparison, the navigation results in the ECEF frame are transformed to the ENU frame, and only the navigation results starting from 1000 s are given since the first 1000 s are used for the initial alignment [30], [31], [32].

A. Experiment Study for Case 1
The initial navigation parameters can be provided accurately by the first 1000-s initial alignment. For the initial covariance of SO, set the attitude, velocity, and position error covariance as  2 , respectively. For invariant error-based models, the initial covariance will suffer a transformation from the initial covariance of SO as described in Section IV-B, so the initial covariance is the same in Euclidian space. In addition, set the inertial sensor bias covariance according to Table II. The following comparisons are based on this configuration.
Figs. 5-7 show the estimated attitude error for different algorithms without VB under Case 1. It can be noted that there is no apparent discrepancy between the five algorithms in terms of pitch, roll, and yaw indices. It can be observed from    7 that when the boat is turning around (at about 1000, 3600, 5500, 6600, and 7900 s), its attitude angle error increases immediately but converges quickly as well. The attitude error, especially the yaw error, will increase to 0.3 • after departure due to intense maneuvers and the inaccurate initial covariance settings, but its error will converge gradually.  and 0.045 • , respectively. LSEGAKF (RSEGAKF), compared to LSEKF (RSEKF), does not significantly exhibit advantages in terms of the yaw estimation although its error propagation follows the log-linear property under group affine error state model. On the one hand, the yaw estimated error can achieve satisfactory results even for pure inertial solution as the high-precision inertial navigation instruments were adopted in our field experimental; in addition, for the SINS/DVL integrated navigation, the yaw is observable and can be well corrected [42].
However, it can be noticed from Table III that the leftinvariant error-based algorithm can achieve a slightly better performance than the right-invariant error-based one.
Figs. 8 and 9 show the velocity and position errors of the different algorithms without VB, respectively. Fig. 10 shows the position error for different algorithms with VB. The quantitative metrics for velocity and position errors are summarized in Tables IV and V, respectively. For the velocity accuracy, the five algorithms remain basically the same, with the MAE of both the east velocity error and the north velocity error not exceeding 0.01 m/s. For the navigation accuracy, LSEGAKF has the best navigation accuracy performance among the compared algorithms, especially for the north position error, which is only 4.97 m. The horizontal position error MAE value of LSEGAKF is 9.80 m, and the PEP value is only 0.053% D, which is the lowest of all. Fig. 10 shows the position errors of the different algorithms, and these algorithms employ the VB approach to infer inaccurate PNCMs and MNCMs. Here, a prefix "VB-" will be  added to the algorithms if the VB approach has been adopted. The detailed quantitative results of the position errors are summarized in Table VI. Fig. 10 and Table VI show that the left-invariant error-based methods are competitive compared to other algorithms, especially the VB-LSEGAKF. Compared with the algorithms without VB, the navigation accuracy of the algorithms with VB is enhanced significantly.
From Tables V and VI, we can conclude that the Hori-Error MAEs of RSEKF (VB-RSEKF) and RSEGAKF (VB-RSEGAKF) are basically the same; besides this, rare improvement is achieved compared with SOKF (VB-SOKF). These results can be attributed to the fact that the RSEKF (VB-RSEKF) error state models are coupled with navigation states, as shown in Tables I and VII. However, it is indisputable that the navigation accuracy of all algorithms is improved by introducing the VB approach.
To further illustrate the effectiveness of our proposed algorithms, the inferred MNCM is fit to the actual MNCM, which is calculated by the reference velocity and DVL measured velocity. As shown in Fig. 11, the inferred MNCM is positively correlated with the actual MNCM, and it is consistent with the movement and is capable of tracking the time-varying noise. From another perspective, the VB approach can somewhat attenuate the negative impact caused by the time-varying or inaccurate MNCM as well as the inaccurate PNCM.
Therefore, it can be concluded that for SINS/DVL integrated navigation, it is favored to select the left-invariant error-based model than the right-invariant error-based model when the error state model needs to augment with IMU drift bias. Moreover, for the left-invariant error-based model, the performance is further improved by introducing the group affine state representation. On the contrary, for the right-invariant error-based  model, the performance does not improve obviously even if the group affine condition is satisfied due to the strongly coupled nature of the navigation parameters.
Remark 9: Since the reference navigation information was available in the ENU frame, the inferred MNCM in the ECEF frame will be transformed to the ENU frame for comparison. In order to obtain the authentic noise more accurately, a smooth operation was conducted to the velocity error calculated by reference velocity and DVL measurements in the ENU frame, and then, the fit is performed. Extrapolated results for the other algorithms are not shown here as it is almost equivalent to VB-LSEGAKF. Fig. 12 shows the navigation results of different algorithms without VB under Case 2 (smooth maneuvering condition, from 4000 to 5500 s, as the red trajectory shown in Figs. 2-4), and Fig. 13 shows the results of different algorithms with VB under Case 2. As shown in the partial enlargement of Fig. 12, the LSEKF (LSEGAKF) outperforms SOKF and RSEKF (RSEGAKF) with slight advantages in terms of Hori-Error and Hori-Error percent. There is rarely a difference between SOKF and RSEKF (RSEGAKF). Although theoretically, RSEKF (RSEGAKF) will be superior to SOKF, but its error state model coupled with estimated states more seriously as the introduction of IMU drift, it may lead to limited performance.

B. Experiment Study for Case 2
After using VB, it can be noted from Fig. 13 that VB-LSEKF (VB-LSEGAKF) has more pronounced advantages compared with VB-SOKF and VB-RSEKF (VB-RSEGAKF) in terms of Hori-Error and Hori-Error percent. The VB approach makes the navigation accuracy of different algorithms improved significantly. However, there is still no evidence to suggest that VB-RSEGAKF (VB-RSEKF) would be better than VB-SOKF. The navigation accuracy of the right-invariant error-based Kalman filter is worse than that of the left-invariant error-based Kalman filter for SINS/DVL applications. The experimental results are consistent with the previous conclusions.

C. Experiment Study for Case 3
Fig. 14 shows the Hori-Error and Hori-Error percent results of different algorithm under intense maneuvering condition (Case 3, from 8000 to 9500 s, as shown in the green trajectory in Figs. 2-4). Fig. 15 shows the results of different algorithms with VB under Case 3. It can be seen that the LSEGAKF achieves a better performance compared with other algorithms without VB. The max Hori-Error value is worse than Case 2, which is consistent with intuition. However, after a period of time, regardless of whether VB is used, the position error seems to be gradually converging. The observability of SINS/DVL system is complex and related to the maneuvering trajectory. Our future work will analyze the global observability of invariant error-based model for SINS/DVL applications.
As shown in Fig. 15, after employing VB into algorithms, the Hori-Error increases more slowly than the corresponding algorithms without VB, especially for VB-LSEGAKF. It can be concluded that the VB can estimate MNCM effectively. In addition, compared with the algorithm without VB, the MAE of Hori-Error for algorithm with VB is reduced by about 40%. Most importantly, the left-invariant error-based Kalman filter has a preferable performance than the traditional Kalman filter and right-invariant error-based Kalman filter.

VI. CONCLUSION
The nonlinear invariant error defined on the Lie group and Lie algebra developed in recent years is widely exploited in the application of navigation, SLAM, and so on. When the equation of state embedded in the Lie group meets group affine property, the propagation of the invariant error is trajectory independent, and the error in the Lie group and algebraic representation satisfies the logarithmic linear property. In this article, extension and comparison for invariant error-based integrated solutions for SINS/DVL in Earth frame have been explored. First, the invariant error propagation model of SINS under nongroup affine and group affine conditions is derived, and the IMU drift is augmented into the invariant error statespace model. Then, the observation equations under different error models are derived. Finally, from a practical perspective, VB empowers the Kalman adaptive capability of the invariant error-based model, which is capable of estimating time-varying noise and inaccurate process noise online.
The different invariant error-based models are compared comprehensively through a boat-mounted lake field trial with different maneuvering conditions. The experimental results show that for the SINS/DVL integrated navigation, the leftinvariant error-based model is preferably in terms of position accuracy, which is consistent with the theoretical analysis. When the group affine condition is not satisfied, the leftinvariant error-based model is still competitive with the right-invariant error-based model following the group affine. These findings are expected to facilitate the selection and establishment of invariant error-based model for SINS/DVL applications.

APPENDIX
A. Exact Derivation of (23) For the attitude error within vector form, its differential iṡ When deriving the above formulas, the approximations (22)  (A5)

B. Exact Derivation of (28)
The derivation of (28) is given as follows: