Research on Transfer Alignment Algorithms Based on SE(3) in ECEF Frame

Abstract

The initial attitude error is challenging to satisfy the requirements of the linear model due to the complex nature of the ocean environment. This presents a challenge in the transfer alignment of the ship. In order to enhance the precision and velocity of ship transfer alignment, as well as to streamline the alignment processes, this paper proposes a transfer alignment methodology based on the Earth-Centered Earth-Fixed (ECEF) frame special Euclidean group (SE(3)) matrix Lie group. After introducing the two navigation states, velocity and attitude, from the ECEF frame into SE(3), the nonlinear inertial navigation system error state model and its corresponding measurement equations are derived based on the mapping relationship between the Lie groups and Lie algebra. The method effectively solves the error problem due to linear approximation in the traditional transfer alignment method, and applies to misalignment angles of arbitrary scale. The simulation results verify the effectiveness and rapidity of the proposed alignment method in the case of arbitrary misalignment angles.

1. Introduction

With the continuous progress of navigation technology, transfer alignment, as an important navigation technology, receives wide attention. Transfer alignment is a kind of initial alignment method under the condition of moving base, and its main idea is to take the high-precision Master Inertial Navigation System (MINS) as the reference and help the Slave Inertial Navigation System (SINS) to complete the initial alignment by comparing the output information of the MINS and the SINS, and through the corresponding filtering algorithm. This technology has important application value in aerospace, missile guidance, and other fields. Its core objective is to achieve high-precision velocity and attitude estimation [1,2]. In the initial alignment of the ship, in order to shorten the alignment time and accuracy, the transfer alignment method is usually used instead of the autonomous alignment method [3,4].

The traditional Kalman filter (KF) for state estimation is widely used in transfer alignment studies. The initial research on transfer alignment is premised on small misalignment angles and employs Kalman filtering based on a linear error model to estimate the misalignment angles [5,6]. However, the sea is a complex dynamic environment, and it is difficult to meet the assumption of small misalignment angles. Therefore, researchers have continued to develop algorithms suitable for large misalignment angle dynamic systems. To solve the problems brought about by the linear error model, a common thought is to linearize the nonlinear function approximately and then conduct Kalman filtering estimation [7,8]. Reference [9] proposes a state matrix extended Kalman filter that transforms the alignment problem into the estimation of the relative attitude matrix between MINS and SINS, without assuming any alignment angles. This method can accurately estimate the attitude of SINS in a fast and efficient manner under random initial misalignment angles. The method uses Taylor series expansion to linearize the nonlinear function and ignores higher-order error information. In response to the large misalignment angle problem, references [10,11,12] use the Unscented Kalman Filter (UKF) and Cubature Kalman Filter (CKF) nonlinear filters to estimate the misalignment angle, but the UKF and CKF, etc., nonlinear filters have the problem of computational efficiency, and may not be usable in scenarios with high real-time requirements. Overall, although the EKF has high computational efficiency, its accuracy in estimating the misalignment angle is severely compromised when the misalignment angle is large. In contrast, the UKF and CKF offer higher accuracy in handling large misalignment angles, but they have low computational efficiency and may not be suitable for scenarios with high real-time requirements.

The rise of Lie group models in navigation applications provides new ideas for the initial alignment of ships [13,14]. When the navigation state defined in the Lie group space satisfies the group affine, the error state model is accurate and can be applied to the alignment of any misalignment angle [15]. The mathematical nature of Lie groups renders them more efficient in handling nonlinear attitude variations and state estimations, especially being applicable in complex dynamic circumstances. In recent years, the application of Lie group models in transfer alignment has emerged gradually. Reference [16] presented a linear transfer alignment algorithm based on SE(2). In this method, the velocity error state equation in the traditional transfer alignment was substituted by the velocity error state equation derived in the Lie group space. The proposed algorithm exhibited excellent performance in the case of large installation error angles. Reference [17], based on the local-level frame, re-derived the error state model in the Lie group space and constructed a new model for transfer alignment under large misalignment angles. Reference [16] did not substitute the attitude error equation with the attitude error state equation derived in the Lie group space, and Reference [17] merely derived the state equation based on the left error model in the Lie group space. This paper presents a transfer alignment method based on the left and right error models in the SE(3) Lie group space of the ECEF frame, which achieves high estimation accuracy of misalignment angles while ensuring operational efficiency. The contributions and content organization of this thesis are as follows.

The second section begins by presenting the related knowledge of Lie groups and Lie algebras, and then expounds on the mechanization of SINS in the ECEF frame. Finally, the SE(3) matrix Lie group navigation state defined based on the ECEF frame is deduced, and it is proven that this state satisfies the group affine characteristic. In the third section, by employing the analytical approach of traditional transfer alignment, the transfer alignment methods LSE-KF based on the left error model and RSE-KF based on the right error model were deduced. The fourth section makes a comparison between the traditional extended Kalman filter (EKF) method and the proposed one using numerical simulation analysis. Ultimately, the fifth section provides a summary of the entire paper.

2. Lie Group Model

2.1. Lie Group and Lie Algebra

Lie groups are a kind of group with continuous nature, and through group operations and manifold structures, they are capable of handling the translation and rotation movements of rigid bodies naturally. The special orthogonal group $SO(3)$ is composed of three-dimensional rotation matrices, whereas the special Euclidean group $SE(3)$ is constituted by the rotational part of $SO(3)$ and the displacement part in three-dimensional space.

$$SO(3)=\left\{R\in {ℝ}^{3\times 3}|R{R}^T=I,\det (R)=1\right\}$$

(1)

$$\left.SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right.\right.\right]\in {ℝ}^{4\times 4}|R\in SO(3),t\in {ℝ}^3\}$$

(2)

where $R$ denotes an arbitrary rotation matrix, $t$ denotes a translation vector, and ${ℝ}^{4\times 4}$ denotes a 4 $\times$ 4 matrix.

Lie groups constitute an algebraic structure. Each Lie group has a corresponding Lie algebra, and the Lie algebra is a description of the local properties of the Lie group. The corresponding Lie algebras for $SO(3)$ and $SE(3)$ are:

$$\mathfrak{s}\mathfrak{o}(3)=\{\varphi \in {ℝ}^3,\varphi \times \in {ℝ}^{3\times 3}\}$$

(3)

$$\left.\left.\mathfrak{s}\mathfrak{e}(3)=\left\{\mathit{\zeta }=\left[\begin{array}{c}\varphi \\ {\rho }_v\end{array}\right.\right.\right]\in {ℝ}^6,{\rho }_v\in {ℝ}^3,\varphi \in \mathfrak{s}\mathfrak{o}(3),(\mathit{\zeta }\times )=\left[\begin{array}{cc}(\varphi \times )& {\rho }_v\\ 0^T& 0\end{array}\right]\in {ℝ}^{4\times 4}\right\}$$

(4)

where $\varphi$ is a three-dimensional rotation vector and ${\rho }_v$ is a three-dimensional translation vector.

The exponential mapping connects Lie groups and Lie algebras, and the transformation between Lie groups and Lie algebras can be realized through exponential mapping. The exponential mapping relationship of $\mathfrak{s}\mathfrak{e}(3)$ can be expressed as:

$$\exp (\mathit{\zeta }\times )=\left[\begin{array}{cc}\exp (\varphi \times )& J{\rho }_v\\ 0_{1\times 3}& 1\end{array}\right]$$

(5)

Among them, $\exp$ denotes the exponential mapping of the matrix, and $\exp (\varphi \times )$ is the exponential mapping relationship of $SO(3)$, that is, the Rodrigues formula. $J$ is the coefficient matrix that results from the linear transformation of the translation vector after undergoing exponential mapping, and it corresponds to the Jacobian matrix of $SO(3)$.

$$J={\displaystyle \frac{\sin \varphi }{\varphi }}{I}_{3\times 3}+\left(1-{\displaystyle \frac{\sin \varphi }{\varphi }}\right)a{a}^{\mathrm{T}}+\left({\displaystyle \frac{1-\cos \varphi }{\varphi }}\right)(a\times )$$

(6)

where $a$ denotes a unit vector.

2.2. SE(3) Lie Group Navigation State Definition

2.2.1. SINS Mechanization in the ECEF Frame

Express the ECEF frame as the E frame, the Earth-Centered Inertial (ECI) frame as the i frame, and the body frame as the b frame. To satisfy the group affine property, in the mechanization of SINS, the attitude is denoted by $C_b^e$, the velocity is denoted by $v_{ib}^e$, which is the projection of the velocity of the b-frame relative to the i-frame in the E-frame, and the position is indicated by $p^e$, which is the projection of the position of the b-frame relative to the i-frame in the E-frame [18].

$${\dot{C}}_b^e=C_b^e\left({\omega }_{ib}^b\times \right)-\left({\omega }_{ie}^e\times \right)C_b^e$$

(7)

$${\dot{v}}_{ib}^e=C_b^e{f}^b-\left({\omega }_{ie}^e\times \right)v_{ib}^e+g^e$$

(8)

$${\dot{p}}^e=v_{ib}^e-\left({\omega }_{ie}^e\times \right)p^e$$

(9)

where ${\omega }_{ib}^b$ denotes the gyroscope output, ${\omega }_{ie}^e$ denotes the Earth’s rotational angular velocity in the e-frame, $f^b$ denotes the accelerometer output, and $g^e$ denotes the Earth’s gravitational acceleration.

2.2.2. Group Affine Analysis

If the group state model satisfies the group affine property, then its error exhibits the characteristic of autonomous estimation. This indicates that the error model is independent of the system state, and the propagation of errors is unaffected by variations in the system state [19]. Compared to the linear approximation error model used in traditional transfer alignment, this model can be applied to transfer alignment with arbitrary misalignment angles, thereby enhancing the performance of transfer alignment.

Incorporate velocity and attitude into the state ${\chi }^e$ of the Lie group space.

$${\chi }^e=\left[\begin{array}{cc}{C}_s^e& v_{is}^e\\ 0_{1\times 3}& 1\end{array}\right]$$

(10)

where $C_s^e$ denotes the attitude of SINS, and $v_{is}^e$ denotes the velocity of SINS. The differential equation of the navigation state is defined as follows

$${\dot{\chi }}^e=f_u\left({\chi }^e\right),{\chi }^e\in G^{n\times n}$$

(11)

where $G^{n\times n}$ denotes the group space. The differential equation for the navigation state ${\chi }^e$ is as follows

$${\dot{\chi }}^e=\left[\begin{array}{cc}{\dot{C}}_s^e& {\dot{v}}_{is}^e\\ 0_{1\times 3}& 0\end{array}\right]$$

(12)

Substituting (7) and (8) into (12) yields

$$\begin{array}{cc}{\dot{\chi }}^e& =\left[\begin{array}{cc}{C}_b^e\left({\omega }_{ib}^b\times \right)-\left({\omega }_{ie}^e\times \right)C_b^e& C_b^e{f}^b-\left({\omega }_{ie}^e\times \right)v_{ib}^e+g^e\\ 0_{1\times 3}& 0\end{array}\right]\hfill \\ & =\left[\begin{array}{cc}{C}_b^e& v_{ib}^e\\ 0_{1\times 3}& 1\end{array}\right]\left[\begin{array}{cc}{\omega }_{ib}^b\times & f^b\\ 0_{1\times 3}& 0\end{array}\right]+\left[\begin{array}{cc}-\left({\omega }_{ie}^e\times \right)& g^e\\ 0_{1\times 3}& 0\end{array}\right]\left[\begin{array}{cc}{C}_b^e& v_{ib}^e\\ 0_{1\times 3}& 1\end{array}\right]\hfill \\ & ={\chi }^e{W}_1+W_2{\chi }^e\hfill \end{array}$$

(13)

$$W_1=\left[\begin{array}{cc}{\omega }_{ib}^b\times & f^b\\ 0_{1\times 3}& 0\end{array}\right],W_2=\left[\begin{array}{cc}-\left({\omega }_{ie}^e\times \right)& g^e\\ 0_{1\times 3}& 0\end{array}\right]$$

(14)

Let ${\chi }_1^e,{\chi }_2^e$ be two distinct state variables in the Lie group space, and $I_d$ denotes the d-dimensional unit matrix corresponding to the state dimension of the Lie group. Substituting (10), (11), and (13) into (15) results in the validity of (15), indicating that (13) complies with the group affine property.

$$f_u({\chi }_1^e{\chi }_2^e)=f_u({\chi }_1^e){\chi }_2^e+{\chi }_1^e{f}_u({\chi }_2^e)-{\chi }_1^e{f}_u(I_d){\chi }_2^e$$

(15)

3. Lie Group Error State Model

3.1. Traditional Transfer Alignment Error Model

The transfer alignment model takes the E-frame as the reference frame. The SINS mechanization in the e frame is given as follows

$$\left\{\begin{array}{c}{\dot{C}}_b^e=C_b^e\left[{\omega }_{eb}^b\times \right]\\ {\dot{v}}_{eb}^e=C_b^e{f}_{ib}^b-2{\omega }_{ie}^e\times v_{eb}^e+g_b^e\\ {\dot{p}}_{eb}^e=v_{eb}^e\end{array}\right.$$

(16)

Due to the brief duration of the transfer alignment process, MINS and SINS remain on the same platform throughout, resulting in their ideal navigation frames being nearly identical. Let s denote the b frame of SINS, and m denote the b frame of MINS. The transfer alignment error model can be formulated as follows:

$$\left\{\begin{array}{c}{\dot{\varphi }}_s^e=-{\omega }_{ie}^e\times {\varphi }_s^e-{\mathit{C}}_s^{\mathrm{e}}\delta {\omega }_{is}^s\\ \delta {\dot{v}}_s^e={\mathit{C}}_s^{\mathrm{e}}{\mathit{f}}^s\times {\varphi }^{\mathrm{e}}-2{\omega }_{ie}^e\times \delta v_s^e+{\mathit{C}}_s^e\delta {\mathit{f}}^s\\ {\dot{\epsilon }}^s=0\\ {\dot{\nabla }}^s=0\\ \dot{\mu }=0\end{array}\right.$$

(17)

where ${\varphi }_s^n$ denotes the misalignment angle of SINS in n the local-level frame, $\delta {\omega }_{is}^s$ denotes the gyroscope measurement error, $f^s$ denotes the accelerometer output, $\delta {\nu }_s^e$ denotes the velocity error, $\delta {\mathit{f}}^s$ denotes the accelerometer measurement error, ${\epsilon }^s$ denotes the constant drift of the SINS gyroscope, ${\nabla }^s$ denotes the constant drift of the SINS accelerometer, and $\mu$ denotes the installation error angle.

The key point of this paper lies in the derivation of the $SE(3)$ transfer alignment error model. To avoid repetition of work, external error sources such as lever arm errors are temporarily not considered. For SINS, only constant drift and random walk are accounted for.

$$\begin{array}{l}\delta {\omega }_{is}^s={\epsilon }^s+w_g^s\\ \delta {\mathit{f}}^s={\nabla }^s+w_a^s\end{array}$$

(18)

It can be derived from (17) that the transfer alignment error state equation is obtained. $W$ and $V$ are the system noise and measurement noise, respectively.

$$X_{sokf}={[{\varphi }_s^e,\delta v_s^e,{\epsilon }^s,{\nabla }^s,\mu ]}^T$$

(19)

$$\left\{\begin{array}{c}{\dot{X}}_{sokf}=F_{sokf}{X}_{sokf}+G_{sokf}W\\ Z_{sokf}=H_{sokf}{X}_{sokf}+V\end{array}\right.$$

(20)

where

$$F=\left[\begin{array}{ccccc}-{\omega }_{ie}^e\times & 0^{3\times 3}& -C_s^e& 0^{3\times 3}& 0^{3\times 3}\\ f^e\times & -2{\omega }_{ie}^e\times & 0^{3\times 3}& C_s^e& 0^{3\times 3}\\ & & & & \\ & & 0^{9\times 15}& & \\ & & & & \end{array}\right]$$

(21)

$$G=\left[\begin{array}{cc}{I}_3& 0_{3\times 3}\\ 0_{3\times 3}& I_3\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$$

(22)

Assuming that the lever arm error between MINS and SINS has been compensated, the difference between the velocities of MINS and SINS in the e-frame can serve as the measurement of velocity.

$$Z_v=v_e^s-v_e^m+V$$

(23)

The product of the direction cosine matrices of MINS and SINS yields the attitude error measurement.

$$Z_{obs}^{att}=C_m^e{\widehat{C}}_n^s=C_s^e{C}_m^s{\widehat{C}}_n^s$$

(24)

where

$$\begin{array}{l}{C}_m^s=(I-\mu \times )\\ {\widehat{C}}_n^s=C_n^s(I+{\varphi }_s^e\times )\end{array}$$

(25)

According to (24) and (25), the attitude error measurement vector $Z_{\varphi }$ can be derived as

$$Z_{\varphi }={\varphi }_s^e-C_s^e\mu +V$$

(26)

Finally, combining (23) and (26) yields the traditional transfer alignment “velocity + attitude” measurement matrix.

$$H_{sokf}={\left[\begin{array}{c}\begin{array}{ccccc}I& 0^{3\times 3}& 0^{3\times 3}& 0^{3\times 3}& -C_s^e\end{array}\\ \begin{array}{ccccc}{0}^{3\times 3}& I& 0^{3\times 3}& 0^{3\times 3}& 0^{3\times 3}\end{array}\end{array}\right]}^T$$

(27)

3.2. Left Error State Model

According to different definitions, the state error model in the Lie group space can be classified as the left error model and the right error model.

$$\begin{array}{l}{\eta }_l={\left({\widehat{\chi }}^e\right)}^{-1}{\chi }^e\\ {\eta }_r={\chi }^e{\left({\widehat{\chi }}^e\right)}^{-1}\end{array}$$

(28)

First, the left error model is derived. Substituting (10) into (28) yields

$${\eta }_l={\left({\widehat{\chi }}^e\right)}^{-1}{\chi }^e=\left[\begin{array}{cc}{\widehat{\mathit{C}}}_e^b{\mathit{C}}_b^e& {\widehat{\mathit{C}}}_e^b({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e)\\ 0_{1\times 3}& 1\end{array}\right]=\left[\begin{array}{cc}\exp \left({\varphi }^b\times \right)& J{{\rho }_v}^b\\ 0_{1\times 3}& 1\end{array}\right]$$

(29)

The first term in Equation (29) can be linearly approximated to the first order to obtain

$${\widehat{\mathit{C}}}_e^b{\mathit{C}}_b^e=\exp \left({\varphi }^b\times \right)\approx I_3+{\varphi }^b\times$$

(30)

According to attitude Error Differential Equation

$${\dot{\varphi }}^b=-\left({\omega }_{ib}^b\times \right){\varphi }^b-\delta {\omega }_{ib}^b$$

(31)

The velocity error, as defined by the left error model, is

$$J{{\rho }_v}^b={\tilde{\mathit{C}}}_e^b({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e)$$

(32)

$g^e$ denotes the gravitational vector, which is related to latitude and altitude. Since transfer alignment is relatively short, the changes in height and position are small, so $g^e$ can be approximated as a constant. By differentiating both sides of Equation (32) with respect to time and substituting Equation (8) into Equation (33), while neglecting the higher-order error terms and the error of gravitational acceleration, it can be obtained that

$$\begin{array}{cc}{\displaystyle \frac{d\left(J{\rho }_v^b\right)}{dt}}\hfill & ={\displaystyle \frac{d\left({\widehat{\mathit{C}}}_e^b\left({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e\right)\right)}{dt}}={\dot{\widehat{\mathit{C}}}}_e^b\left({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e\right)+{\widehat{\mathit{C}}}_e^b\left({\dot{\mathit{v}}}_{ib}^e-{\dot{\widehat{\mathit{v}}}}_{ib}^e\right)\hfill \\ \hfill & =-{\widehat{\omega }}_{eb}^b{\widehat{\mathit{C}}}_e^b\left({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e\right)+{\widehat{\mathit{C}}}_e^b\left[f^e-\left({\omega }_{ie}^e\times \right){\mathbf{v}}_{ib}^e+{\mathbf{g}}^e-{\widehat{f}}^e+\left({\omega }_{ie}^e\times \right){\widehat{\mathbf{v}}}_{ib}^e-{\widehat{\mathbf{g}}}^e\right]\hfill \\ \hfill & =\left({\widehat{\omega }}_{ie}^b\times -{\widehat{\omega }}_{ib}^b\times \right){\widehat{\mathit{C}}}_e^b\left({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e\right)+{\widehat{\mathit{C}}}_e^b\left(f^e-{\widehat{f}}^e\right)+{\widehat{\mathit{C}}}_e^b\left({\omega }_{ie}^e\times \right)\left({\widehat{\mathbf{v}}}_{ib}^e-{\mathbf{v}}_{ib}^e\right)+{\mathbf{g}}^e-{\widehat{\mathbf{g}}}^e\hfill \\ \hfill & =\left({\widehat{\omega }}_{ie}^b\times -{\widehat{\omega }}_{ib}^b\times \right){\widehat{\mathit{C}}}_e^b\left({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e\right)+{\widehat{\mathit{C}}}_e^b\left(f^e-{\widehat{f}}^e\right)+{\widehat{\mathit{C}}}_e^b\left({\omega }_{ie}^e\times \right){\widehat{\mathit{C}}}_b^e{\widehat{\mathit{C}}}_e^b\left({\widehat{\mathbf{v}}}_{ib}^e-{\mathbf{v}}_{ib}^e\right)+{\mathbf{g}}^e-{\widehat{\mathbf{g}}}^e\hfill \\ \hfill & =\left({\widehat{\omega }}_{ie}^b\times -{\widehat{\omega }}_{ib}^b\times \right){\widehat{\mathit{C}}}_e^b\left({\mathit{v}}_{ib}^e-{\widehat{\mathit{v}}}_{ib}^e\right)+{\widehat{\mathit{C}}}_e^b\left(f^e-{\widehat{f}}^e\right)+\left({\omega }_{ie}^b\times \right){\widehat{\mathit{C}}}_e^b\left({\widehat{\mathbf{v}}}_{ib}^e-{\mathbf{v}}_{ib}^e\right)+{\mathbf{g}}^e-{\widehat{\mathbf{g}}}^e\hfill \\ \hfill & =-{\widehat{\omega }}_{ib}^b\times J{\rho }_v^b-{\widehat{\mathit{C}}}_e^b\left({\widehat{f}}^e+\delta f^e\right)+{\mathbf{g}}^e-{\widehat{\mathbf{g}}}^e\hfill \\ \hfill & =-{\widehat{\omega }}_{ib}^b\times J{\rho }_v^b-{\widehat{f}}^b\times {\varphi }^b-\delta f^b\hfill \end{array}$$

(33)

At this point, the state equation can be written as

$${\dot{X}}_{lse}=F_{lse}{X}_{lse}+G_{lse}W$$

(34)

where

$$X_{lse}={[{\varphi }_s^b,J{{\rho }_v}^b,{\epsilon }^s,{\nabla }^s,\mu ]}^T$$

(35)

$$F_{lse}=\left[\begin{array}{ccccc}-{\widehat{\omega }}_{ib}^b\times & 0^{3\times 3}& -I_3& 0^{3\times 3}& 0^{3\times 3}\\ -{\widehat{f}}^b\times & -{\widehat{\omega }}_{ib}^b\times & 0^{3\times 3}& -I_3& 0^{3\times 3}\\ & & 0^{6\times 15}& & \end{array}\right]$$

(36)

$$G_{lse}=\left[\begin{array}{cc}-I_3& 0_{3\times 3}\\ 0_{3\times 3}& -I_3\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$$

(37)

Because the accuracy of MINS is much higher than that of SINS, the MINS error is usually ignored. Thus, (30) can be written as

$${\widehat{\mathit{C}}}_e^b{\mathit{C}}_b^e={\widehat{\mathit{C}}}_e^s{\mathit{C}}_m^e$$

(38)

Substituting (25) into (38) and disregarding higher-order error terms, it can be obtained that

$$\begin{array}{cc}{Z}_{lse-mat}& ={\widehat{\mathit{C}}}_e^s{\mathit{C}}_m^e\hfill \\ & ={\mathit{C}}_e^s\left(I+{\varphi }_s^e\times \right){\mathit{C}}_s^e{C}_m^s\hfill \\ & ={\mathit{C}}_e^s\left(I+{\varphi }_s^e\times \right){\mathit{C}}_s^e\left(I-\mu \times \right)\hfill \\ & =I-\mu \times +{\varphi }_s^b\times \hfill \end{array}$$

(39)

The attitude measurement matrix $Z_{lse\varphi }$ is constructed based on Equation (39).

$$Z_{lse\varphi }=\left[\begin{array}{c}{Z}_{lse-mat}(2,3)\\ Z_{lse-mat}(3,1)\\ Z_{lse-mat}(1,2)\end{array}\right]={\varphi }_s^b-\mu$$

(40)

Finally, the “velocity + attitude” matching measurement equations are constructed in accordance with Equations (32) and (40).

$$Z_{lse}=H_{lse}{X}_{lse}+V$$

(41)

where

$$H_{lse}={\left[\begin{array}{c}\begin{array}{ccccc}I& 0^{3\times 3}& 0^{3\times 3}& 0^{3\times 3}& -I\end{array}\\ \begin{array}{ccccc}{0}^{3\times 3}& I& 0^{3\times 3}& 0^{3\times 3}& 0^{3\times 3}\end{array}\end{array}\right]}^T$$

(42)

3.3. Right Error State Model

Similar to Section 3.2, by substituting (10) into (28), the right error model can be expressed as

$${\eta }_r={\chi }^e{\left({\widehat{\chi }}^e\right)}^{-1}=\left[\begin{array}{cc}{C}_b^e{\widehat{C}}_e^b& v_{ib}^e-C_b^e{\widehat{C}}_e^b{\widehat{v}}_{ib}^e\\ 0_{1\times 3}& 1\end{array}\right]=\left[\begin{array}{cc}\exp \left({\varphi }^e\times \right)& J{{\rho }_v}^e\\ 0_{1\times 3}& 1\end{array}\right]$$

(43)

The first term of Equation (43) can be approximated linearly to obtain

$${\mathit{C}}_e^b{\widehat{\mathit{C}}}_b^e=\exp \left({\varphi }^e\times \right)\approx I_3+{\varphi }^e\times$$

(44)

According to attitude Error Differential Equation

$${\dot{\varphi }}^e=-\left({\omega }_{ie}^e\times \right){\varphi }^e-{\widehat{C}}_b^e\delta {\omega }_{ib}^b$$

(45)

The velocity error, as defined by the right error model, is

$$J{{\rho }_v}^e=v_{ib}^e-C_b^e{\widehat{C}}_e^b{\widehat{v}}_{ib}^e$$

(46)

The analysis is similar to that of (33), and the differential equation for the velocity error from (46) can be expressed as

$${\displaystyle \frac{d\left(J{{\rho }_v}^e\right)}{dt}}=\left({\widehat{g}}^e\times \right){\varphi }^e-\left({\omega }_{ie}^e\times \right)\left(J{{\rho }_v}^e\right)-\left({\widehat{v}}_{ib}^e\times \right)\left({\widehat{C}}_b^e\right)\delta {\omega }_{ib}^b-{\widehat{C}}_b^e\delta f^e$$

(47)

The state equation can be written as

$${\dot{X}}_{rse}=F_{rse}{X}_{rse}+G_{rse}W$$

(48)

where

$$X_{rse}={[{\varphi }_s^e,J{{\rho }_v}^e,{\epsilon }^s,{\nabla }^s,\mu ]}^T$$

(49)

$$F_{rse}=\left[\begin{array}{ccccc}-{\omega }_{ie}^e\times & 0^{3\times 3}& -{\widehat{C}}_b^e& 0^{3\times 3}& 0^{3\times 3}\\ {\widehat{g}}^e\times & -{\omega }_{ie}^e\times & -\left({\widehat{v}}_{ib}^e\times \right){\widehat{C}}_b^e& -{\widehat{C}}_b^e& 0^{3\times 3}\\ & & 0^{6\times 15}& & \end{array}\right]$$

(50)

$$G_{rse}=\left[\begin{array}{cc}-{\widehat{C}}_b^e& 0_{3\times 3}\\ -\left({\widehat{v}}_{ib}^e\times \right){\widehat{C}}_b^e& -{\widehat{C}}_b^e\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$$

(51)

$Z_{rse-mat}$ constitutes the product of the MINS attitude matrix and the SINS attitude matrix.

$$\begin{array}{cc}{Z}_{rse-mat}& ={\mathit{C}}_m^e{\widehat{\mathit{C}}}_e^s\hfill \\ & ={\mathit{C}}_s^e{\mathit{C}}_m^s{\mathit{C}}_e^s\left(I_3+{\varphi }^e\times \right)\hfill \\ & ={\mathit{C}}_s^e(I-\mu \times ){\mathit{C}}_e^s\left(I_3+{\varphi }^e\times \right)\hfill \\ & =I-{\mu }^e\times +{\varphi }^e\times \hfill \end{array}$$

(52)

Construct the attitude measurement matrix $Z_{rse\varphi }$ according to Equation (52).

$$Z_{rse\varphi }=\left[\begin{array}{c}{Z}_{rse-mat}(2,3)\\ Z_{rse-mat}(3,1)\\ Z_{rse-mat}(1,2)\end{array}\right]={\varphi }_s^e-{\mu }^e$$

(53)

According to (46) and (53), the “velocity + attitude” matching measurement equation $Z_{rse}$ can be constructed.

$$Z_{rse}=H_{rse}{X}_{rse}+V$$

(54)

where

$$H_{rse}={\left[\begin{array}{c}\begin{array}{ccccc}I& 0^{3\times 3}& 0^{3\times 3}& 0^{3\times 3}& -{\widehat{C}}_s^e\end{array}\\ \begin{array}{ccccc}{0}^{3\times 3}& I& 0^{3\times 3}& 0^{3\times 3}& 0^{3\times 3}\end{array}\end{array}\right]}^T$$

(55)

4. Simulation

In the transfer alignment procedure, the error of navigation parameters is usually estimated using a Kalman filter, and the SINS is corrected with the estimated error, as shown in Figure 1. In order to analyze the performance of LSE-KF and RSE-KF under arbitrary misalignment angles, simulation experiments under different misalignment angle conditions were conducted in Section 4. The experiments were conducted using MATLAB 9.12.0.1884302 (R2022a). The computer used for the experiments was equipped with 32 GB of RAM and a 2.3 GHz Intel Core i7-12700H processor.

The error set in Reference [20] is related to the actual error characteristics of the ship and has a certain representativeness. To ensure the credibility of the experimental results, this study adopted the conditions specified in Reference [20]. The initial position was set to 30° N, 113° E, with an initial attitude of $[0° 0° 0°]$. The misalignment angles were configured as $[0.2° 0.3° 1.2°]$ for the small misalignment condition and $[30° 30° 60°]$ for the large misalignment condition. The simulation time is 40 s, and the SINS sampling frequency is 100 Hz.

Inertial device performance of the SINS: the constant drift of the gyroscope and the accelerometer are $10°/\mathrm{h}$ and $1000 \mathsf{\mu }\mathrm{g}$, respectively, and the random walk of the gyroscope and the accelerometer are $0.01°/\sqrt{\mathrm{h}}$ and $10 \mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{H}\mathrm{z}}$, respectively. The installation error angle is $[{30}^{\prime } {10}^{\prime } {20}^{\prime }]$. In practice, ships are subject to wave and wind effects. Commonly, a three-axis swaying motion is employed to simulate the shipboard environment [21]. The swing angles are set as shown in

Table 1. Swing motion parameter setting.

Swinging Angular	Amplitudes/(°)	Periods/(s)	Initial Phase/(°)
Pitch	3°	12 s	0°
Roll	2°	10 s	0°
Yaw	4°	15 s	0°

.

Figure 2, Figure 3 and Figure 4 show the attitude error curves under the initial misalignment angle of $[0.2° 0.3° 1.2°]$. To demonstrate convergence accuracy,

Table 2. RMSE in small misalignment cases.

Method	${\mathit{\varphi }}_{\mathit{E}} \mathbf{(}\mathbf{\prime }\mathbf{)}$	${\mathit{\varphi }}_{\mathit{N}} \mathbf{(}\mathbf{\prime }\mathbf{)}$	${\mathit{\varphi }}_{\mathit{U}} \mathbf{(}\mathbf{\prime }\mathbf{)}$
EKF	0.20′	0.14′	0.31′
LSE-KF	0.07′	0.14′	0.18′
RSE-KF	0.11′	0.16′	0.05′
UKF	0.04′	0.45′	0.08′

statistically shows the root mean square error (RMSE) for the 30–41 s time period. The results indicate that in the case of small misalignment angles, all three methods can achieve convergence. Among them, RSE-EF fluctuates greatly before convergence, but its accuracy after convergence is higher than that of the EKF and is comparable to the convergence accuracy of the LSE-EF. The LSE-EF has the fastest convergence rate, while the RSE-EF has a slower convergence rate. Under the swinging condition, the convergence of the LSE-EF method’s misalignment angle estimate value can be achieved within about 5 s, with a convergence accuracy of less than 0.2′.

Figure 5, Figure 6 and Figure 7 show the attitude error curves under the initial misalignment angle of $[30° 30° 60°]$.

Table 3. RMSE in large misalignment case.

Method	${\mathit{\varphi }}_{\mathit{E}} (\prime )$	${\mathit{\varphi }}_{\mathit{N}} (\prime )$	${\mathit{\varphi }}_{\mathit{U}} (\prime )$
EKF	18.52′	18.89′	11.64′
LSE-KF	4.64′	9.80′	5.15′
RSE-KF	8.12′	7.39′	5.30′
UKF	5.63′	8.58′	9.12′

statistically shows the RMSE for the 30–41 s time period. Under large misalignment angle conditions, both the LSE-KF and RSE-KF demonstrate faster convergence rates compared to the EKF, starting to converge around 10 s. Additionally, the estimation accuracy of both LSE-KF and RSE-KF is superior to that of the EKF. As shown in Table 3, both the LSE-KF and RSE-KF exhibit convergence accuracies of less than 10′, which are higher than that of the EKF. This indicates that LSE-KF and RSE-KF demonstrate better convergence accuracy and speed under conditions of large misalignment angles, consistent with the experimental conclusions.

5. Vehicle Experiment

This chapter validates and evaluates the LSE-KF and RSE-KF models by analyzing the GNSS/INS dataset of the vehicle. The vehicle trajectory is shown in Figure 8, where the red mark indicates the starting point, and the entire operation duration is 140 s.

Table 4. Reference noise parameters of HGuide-i30.

IMU	Angle Random Walk	Velocity Random Walk	Gyroscope-Bias Standard Deviation	Accelerometer-Bias Standard Deviation
HGuide-i300	$0.2\mathrm{deg}/\sqrt{\mathrm{h}}$	$0.2\mathrm{m}/\mathrm{s}/\sqrt{\mathrm{h}}$	$200\mathrm{deg}/\mathrm{h}$	$1000\mathrm{mGal}$

shows the device error level of IMU1 (model: HGuide-i300, Manufacturer: Honeywell, Charlotte, NC, USA) and takes it as the salve inertial navigation system. The original data update cycle of IMU1 is 200 Hz. Figure 9 shows the real attitude and velocity reference of IMU2 (model: ADIS16465, Manufacturer: Analog Devices, Cambridge, MA, USA) in the data set, taking IMU2 as the master inertial navigation information source, and its output frequency is 1 Hz. From the attitude change, it can be observed that there is a turning movement approximately between 50–60 s and 100–120 s. Consistent with the simulation experiment, this study adopts the “speed + attitude” matching method to compare the accuracy and running time of several different transfer alignment methods.

The experiment will be carried out under the conditions of small misalignment angles and large misalignment angles. The small misalignment angles are set as $[0.2°, 0.3°, 1.2°]$, and the large misalignment angles are set as $[30°, 30°, 60°]$. The attitude estimation error curves of transfer alignment are shown in Figure 10 and Figure 11.

Table 5. RMSE in small misalignment cases during vehicle experiments.

Method	${\mathit{\varphi }}_{\mathit{E}} (\prime )$	${\mathit{\varphi }}_{\mathit{N}} (\prime )$	${\mathit{\varphi }}_{\mathit{U}} (\prime )$
EKF	1.97′	2.77′	1.82′
UKF	0.81′	2.01′	7.67′
LSE-KF	3.50′	1.15′	1.90′
RSE-KF	2.51′	2.90′	3.99′

and

Table 6. RMSE in large misalignment cases during vehicle experiments.

Method	${\mathit{\varphi }}_{\mathit{E}} (\prime )$	${\mathit{\varphi }}_{\mathit{N}} (\prime )$	${\mathit{\varphi }}_{\mathit{U}} (\prime )$
EKF	11.44′	9.00′	36.96′
UKF	2.33′	5.21′	1.88′
LSE-KF	0.98′	0.69′	6.91′
RSE-KF	1.38′	4.65′	1.49′

are the RMSE of misalignment angle estimation within 130–140 s.

As shown in Figure 10, under the condition of small misalignment angles, the misalignment angle errors of the four transfer alignment methods will converge rapidly after two turning maneuvers. As indicated in Table 5, the convergence accuracy of the misalignment angle for all four transfer alignment methods is within 8′, showing that their convergence performance is favorable. As shown in Figure 11, within the initial 10 s of the vehicle’s straight-line travel, the estimation of the misalignment angle gradually converges due to the influence of speed error. Subsequently, at 50 s, a steering maneuver occurs, significantly enhancing the observability of the misalignment angle and thereby improving the accuracy of its estimation. Eventually, after the second steering maneuver, the estimation of the misalignment angle achieves complete convergence. From Table 6, it can be seen that the convergence accuracy of EKF is relatively low, while the accuracy of LSE-KF and RSE-KF is slightly higher than that of UKF.

Table 7. The total running time of the four transfer alignment algorithms.

Method	Total Running Time
EKF	4.68 s
UKF	25.38 s
LSE-KF	8.43 s
RSE-KF	9.8 s

shows the total running time of different transfer alignment methods. The results indicate that although the LSE-KF and RSE-KF methods increase the computing time by about 4–5 s compared to EKF, they are still significantly lower than the UKF method. Therefore, LSE-KF and RSE-KF are more suitable for transfer alignment under large misalignment angles, and the computational burden is relatively smaller.

6. Conclusions

This paper derives two transfer alignment methods, LSE-KF and RSE-KF, based on different error state models. First, the velocity and attitude in the ECEF coordinate system are defined in the Lie group space, forming the SE(3) matrix Lie group navigation state. Then, it is proven that the Lie group state satisfies the group affine property. Subsequently, based on the left error model and the right error model, the paper proposes the LSE-KF and RSE-KF transfer alignment methods within the framework of the E-frame Lie group space SE(3), effectively addressing the linear approximation error issues present in traditional models. Finally, through simulation experiments and vehicle experiments, the LSE-KF and RSE-KF methods were compared with the EKF method. The results show that in the case of small misalignment angles, the proposed method has slightly better convergence speed and convergence accuracy than the EKF method, and has comparable accuracy to the UKF method. In the case of large misalignment angles, the proposed method can achieve higher convergence accuracy without significantly increasing the computation time. The experimental results show that the proposed method has higher estimation accuracy and faster convergence speed in any misalignment angle, which helps to improve the quality of ship transfer alignment. The method proposed in this paper avoids the accuracy loss caused by the linearization approximation of the nonlinear function of the attitude error model and the computational efficiency problem of the nonlinear filter. It has a certain development space in practical applications. In future work, other matching algorithms can be explored, and the influence of flexural deformation and lever arm error on the algorithm should also be considered.