#### Abstract

In order to solve the coarse alignment problem of the strapdown inertial navigation system on a rocking base, a fast coarse alignment method using the Special Orthogonal Group optimization has been proposed in this paper. In this method, based on the alignment idea of tracing gravitational apparent motion in inertial frame, the model of coarse alignment on a rocking base has been established using the Special Orthogonal Group directly. A new attitude error function has been proposed on the basis of the cosines between the measurement vector and predictive vector to describe the error between the estimated attitude and the true one. In order to directly reflect the change in the attitude error in the new innovation term and enable the attitude error to converge to zero as fast as possible, the gradient of the new attitude error function has been selected as the new innovation term to compensate for the attitude in the estimation process. Finally, the stability of the proposed optimization estimation method has been proved by employing the Lyapunov stability theory. Simulation and experiment results show that the method presented in this paper exhibits good performance in terms of alignment accuracy and time and can be applied to coarse alignment under a rocking base under different environments.

#### 1. Introduction

The strapdown inertial navigation system (SINS) involves plenty of knowledge of different subjects and has developed into a cutting-edge technology in the scientific world [1]. For SINS, the initial speed and position can be obtained using auxiliary sensors. The initial attitude is determined by initial alignment which is a key technology for the research of SINSs [2]. The convergence precision and speed of the initial alignment directly determine the performance of the SINSs [3].

A two-step alignment framework including coarse alignment and fine alignment is usually adopted in most of the initial alignment methods. The purpose of the coarse alignment is to obtain an accurate initial attitude in a short time. Fine alignment linearizes the attitude model based on the relatively accurate attitude obtained from the coarse alignment [4]. If the precision of the coarse alignment is insufficient, there will be large linearization error and, more serious, the system may not be linearized. Therefore, the coarse alignment plays an important role in the whole initial alignment process. In the case of static bases, the coarse alignment has been addressed well, and several common methods such as analytic rough alignment [5] and zero speed alignment based on the Kalman filtering [6] have been proposed in the literature. These coarse alignment methods use two Earth feature vectors, namely, the acceleration of gravity and the self-rotational angular rate of the Earth to calculate the attitude matrix. However, for the rocking base, there are some interference factors, such as engine rotation, wind, and wave surging [7], which give rise to an issue that the angular velocity of the disturbance far outweigh the angular velocity of the Earth’s rotation. Although some methods have been proposed to solve this problem, the convergence accuracy and rate do not satisfy the requirements of the carrier.

In recent years, a coarse alignment method based on the gravitational apparent motion in the inertial frame for SINS on a rocking base has been investigated [8, 9]. The inertial frame coarse alignment methods can be divided into two categories, namely, the dual-vector determination method [10–13] and the Wahba method [14–17]. The former one is based on the traditional dual-vector method used for calculating the initial attitude. According to the principle of dual-vector attitude determination, the TRIAD algorithm is proposed and gravity is used as an observation vector [10]. The attitude matrix is calculated using two noncollinear gravity vectors at different times. An improved alignment method based on the inertial frame was proposed by Silson in [11]. The velocity was obtained by integrating its apparent motion in gravity so as to reduce the influence of random noise on the measured acceleration. Owing to the errors in the measured value of the sensors, a digital low-pass filter was developed to reduce the high-frequency noise in the sensor measurement [12, 13]. It was shown that the accuracy of the coarse alignment had been improved and an accurate acceleration value could be achieved. However, the dual-vector determination method only uses the gravity vector of two moments to determine the initial attitude, and a large amount of measurement information is discarded, resulting in a poor precision of the coarse alignment process. Furthermore, the method requires that the two gravity vectors used for calculating the attitude are noncollinear, and thus, the time interval between the two selected gravity vectors is required to be sufficiently long. This implies that the alignment time of the coarse alignment process based on dual-vector determination will take longer.

The second method is to transform the traditional attitude estimation problem into the Wahba problem and obtain the initial attitude using different solving methods [14–17]. For the coarse alignment problem under a rocking base, an algorithm based on the singular value decomposition (SVD) was proposed by Jiang and Wang [14]. This algorithm solves the Wahba problem by using the theory of SVD to obtain the direction cosine matrix (DCM). However, its disadvantage is that it takes a long time to converge. A coarse alignment method based on Q-method was described by Yang in [15]. In this method, a unit quaternion was used for solving the Wahba problem, which can directly obtain the attitude quaternion and reduce the computation by avoiding solving DCM. A new coarse alignment method based on quaternion estimation (QUEST) was developed by Cheng and Shuster in [16]. This method transforms the Wahba problem into an eigenvalue determination problem. The characteristic polynomial is given, and the eigenvalues can be obtained by Newton’s method after several iterations. In [17], an extension of the work done by Cheng and Shuster [16] was proposed, where the main focus was the simplification of the characteristic polynomials. This method, named as the fast linear attitude estimator (FLAE), gives a symbolic solution of the characteristic polynomial, making the process of calculating the eigenvalue simpler and faster. Although the Wahba method can make full use of all the measurement information, it is nonconvex and not guaranteed to be globally optimal and this will affect the alignment accuracy and rate during the coarse alignment process.

In recent years, a new class of attitude estimation methods has been developed based on the Special Orthogonal Group representation, defined as -based estimation method. Different from the quaternion, there are three distinctive advantages in these estimation methods [18, 19]: (i) they are deterministic in that they require no knowledge of the noise properties of the sensor; (ii) since they evolve directly on the Special Orthogonal Group of rigid-body rotations, they uniquely describe the attitude of a rigid body; and (iii) the convergence properties and the stability of these methods can be guaranteed. Because of these advantages, the -based estimation method can be used for attitude estimation and control of various carriers by combing the existing method, such as ships, aircraft, vehicle, mobile robots, and quadrotors [20–23]. In the work by Khosravian and Namvar [24], the distance of the attitude error group to the identity matrix, defined as F-norm, was selected as the attitude error function. Then, this function was used as a Lyapunov candidate function to prove the stability of the system. Zlotnik and Forbes [25] used the Euclidean distance of the measurement vector and the estimation vector as the attitude error function and established the system renewal equation using the group to realize attitude estimation. A particular attitude error function that uses the trace of the attitude error to the identity matrix to measure the error between the estimated attitude and the true one was used by Grip et al. [26]. An adaptive identifier on Special Orthogonal Group (AISOG) was proposed by Kinsey and Whitcomb in [27]. In these works, the cross product of the vector was used as an innovation term to compensate the attitude and the asymptotic stability of the system was proved. However, in the existing -based estimation methods, there is no direct relationship between the innovation term and the attitude error function. Thus, it is difficult to achieve the reflection of the change of the attitude error function in the update term, which results in poor performance of attitude estimation in terms of convergence time and convergence accuracy.

Motivated by the advantages of the -based estimation methods, this paper proposes a rapid coarse alignment method using the Special Orthogonal Group optimization for SINS on a rocking base. On the basis of the attitude decomposition and the Special Orthogonal Group differential equation, a coarse alignment model based on the group has been established. A new attitude error function has been established to efficiently represent the attitude error by using the relationship between vectors. In order to reflect the change in the attitude error in the attitude compensation, the gradient of the new attitude error function has been used as the innovation term to compensate the attitude in real time, which enables the system to converge quickly. The Lyapunov stability theory has been used to prove the stability of the proposed optimization method. Simulation and experiments were designed to verify the performance in terms of alignment accuracy and time as compared to the existing coarse alignment methods.

The main contributions of this paper can be summarized as follows: (1) the attitude error function is defined as the cosine similarity between the true reference vector and estimated reference vector, which can establish the direct relationship between the innovation term and the attitude error function. (2) In order to reflect the change of the attitude error function and improve the rate of converge of the attitude error function, the gradient of the attitude error function is used as the update term to compensate the estimated attitude. (3) The stability of the proposed method is proved by Lyapunov’s second theorem, which gives an effective way to prove the stability of Special Orthogonal Group optimization method.

The remainder of this article is organized as follows: Section 2 provides a review of the preliminary material. The definitions of the reference frames and various symbols are listed in Tables 1 and 2, respectively. A description of the system model based on the Special Orthogonal Group representation is given in Section 3. Details of the proposed Special Orthogonal Group optimization method are given in Section 4, and the stability analysis is given in Section 5. The results of the simulation and experiment are given in Section 6. Conclusions from this work are given in Section 7.

#### 2. Preliminaries

A Special Orthogonal Group is a smooth and differentiable manifold satisfying two conditions: group operations and reversibility [18]. The Special Orthogonal Group describes the group of 3D rotation matrices and is defined as

Special Orthogonal Group forms a smooth manifold, and its tangent space at the origin is a skew-symmetric matrix, denoted as [28],

The operation represents the mapping from a vector to the corresponding skew-symmetric matrix :

The inverse operation represents the map from the skew-symmetric matrix to a vector .

The operation defines the exponential map from to and conforms to the standard matrix exponential formula (the Rodrigues’ formula):

If , then and are skew-symmetric matrices [28]. The Special Orthogonal Group differential equation can then be described as where . For a given sampling time interval , the discrete implementation of (6) is given as

The tangent space of the group at any point is given by [29]

The inner product on is defined as

The Riemannian metric on the group can be defined as [29]

The gradient of a function is defined as the unique tangent vector such that [29] where and is a curve around . is the tangent map of at.

#### 3. The System Model Based on the Special Orthogonal Group Representation

Because of the nature and the advantages of the group, representation can be used to improve the performance of the coarse alignment on a rocking base. The alignment model based on the Special Orthogonal Group differential equation and the idea of the inertial frame alignment has been established in this section.

##### 3.1. State Model

According to the attitude chain rule, the initial attitude matrix can be decomposed into a product of three Special Orthogonal Groups: where , represents the attitude matrix of the frame relative to the frame, and is the initial attitude group and represents the attitude change from frame to frame at the initial moment. and are time-varying attitude groups in the frame or frame, respectively, from time to 0 and can be calculated using the following equations: where is the angular velocity of the frame relative to the frame under the frame and is the angular velocity of the frame relative to the frame under the frame and can be calculated by the expression , where is the angular velocity of the rotation of the Earth, is the local latitude, and is the zero vector when the carrier is not moving.

Using (5), and can be obtained as where , , and is the sampling time. is calculated using the two-sample iteration algorithm , where and are the first and second samples of the gyro-measured incremental angle [30].

Because and are the attitude matrices at the initial moment, they can be chosen as unit matrix. Then, the expressions for and in the iteration calculation are given as follows:

Since and are calculated using (17) and (18), can be obtained from (12) if the initial attitude group can be determined. Then, the coarse alignment problem is transformed into the problem of estimating the initial attitude group . Because does not change with time, the state equation based on the Special Orthogonal Group representation can be established as

##### 3.2. Measurement Model

In some inertial frame alignment methods, the accelerometer measurements at different moments are directly used for calculating the gravitational apparent motion. As a result, the alignment accuracy is easily disturbed by the measurement noises, especially the random noises from the accelerometers [10, 11, 14]. To reduce the influence of the measurement noises, previous work [9, 12] integrated the gravitational acceleration with time in order to obtain the velocity and then used the velocity vectors instead of the gravity vectors for the alignment calculation. Similar to the existing inertial frame alignment methods, we also use the velocity vectors to establish the measurement equation. Thus, the measurement equation based on the Special Orthogonal Group can be established as

The measurement equation can then be written as where

The coarse alignment model based on the Special Orthogonal Group representation can be established as

Because the state of this model is the group, the alignment problem is transformed into the problem of estimating the initial attitude group . Furthermore, it can be seen from (24) that the alignment model established on the basis of the Special Orthogonal Group is a linear model that effectively avoids the nonlinear problem and the nonuniqueness of the unit quaternion representation.

#### 4. The Special Orthogonal Group Optimization Method

For the model expressed by (24), a novel estimation method is proposed to improve the alignment accuracy and time, and its details are presented in this section. In the model represented by (24), the state is the initial attitude group , while the attitude error function of is usually described as

Then, through the definition of the attitude error function, the estimated value could be adjusted repeatedly so that the attitude error approaches the identity matrix . And is considered to be the optimal estimate of .Generally, the gradient descent method can quickly reduce the value of this attitude error function to the minimum value . However, since the unknown attitude truth value is involved, the obtained gradient should be a function of the unknown value , which makes the gradient descent method unable to be used in the alignment algorithm using the traditional attitude error function and affects the rapidity of local error descent. Therefore, a new error function is needed, which can be consistent with the traditional error function in the geometric sense, but the update of this error function should not be subject to the unknown truth value so that the gradient is solvable, and the gradient could be used to continuously adjust estimated value to make the error function converge quickly.

In order to efficiently reflect the changes of the attitude error in the coarse alignment on a rocking base, a new attitude error function based on the cosine between the vectors has been proposed in the novel estimation method. Here, the gradient of the new attitude error function is taken as the innovation term in the estimation process. The proposed method can enable the estimated attitude to spread along the gradient descent direction of the new attitude error function such that the attitude error approaches to zero.

For the measurement equation given by (24), the Special Orthogonal Group can convert the vector in the *n* frame into the vector in the *b* frame. If the projection of the vector direction is considered individually, Equation (20) can be written as
where , , and thus, and are both unit vectors.

In the estimation process, if the estimated attitude is equal to the real attitude , the measurement vector and predictive vector should be equal. However, since there are errors between and , and are not equal. Therefore, in order to accurately describe the error of the attitude estimation, the following attitude error function has been constructed:

Since and are both unit vectors, Equation (23) can be written as

Using (25), (27) can be rewritten as

In order to improve the convergence speed of the estimation of attitude, the gradient of the new attitude error function is used as the innovation term for updating the attitude in the estimation process. Some preliminary knowledge needs to be introduced before deriving the gradient of the attitude error function.

According to [29], the infinitesimal variation of is

For any , the following equation holds: where and . The operator is an antisymmetric projection operator and is a symmetric projection operator.

From the definition of in (28), the tangent map of at is given by [29]

Using (29), Equation (28) can be written as

From (30), we have:

The trace of the product of an arbitrary symmetric matrix and an antisymmetric matrix is equal to zero. Thus, where .

Equation (31) can be rewritten using (9) as

On the basis of (10), we can get

From (11), the tangent mapping of has the following relationship with the gradient of :

Since (36) and (37) represent the tangent mapping of , it holds true that where is the gradient of the attitude error function.

We choose as the new innovation term to compensate the attitude group, and this enables the estimated spread of the attitude group to be along the direction of the gradient descent and eventually enables the attitude group to converge fast. Therefore, the entire process of the Special Orthogonal Group Optimization method can be written as follows:

#### 5. Stability of the Special Orthogonal Group Optimization Method

Stability is an important property of a control system. In the linear control system theory, the stability analysis of a mathematical model based on a controlled object is an indispensable content in the system design. In the fields of mathematics and automatic control, Lyapunov’s theorem is often used to judge the stability of a dynamic system. If the trajectory of any initial condition of the dynamical system eventually converges to the equilibrium point, the system can be said to be Lyapunov asymptotically stable at the equilibrium point. In this section, we use the Lyapunov stability theorem to prove that the Special Orthogonal Group optimization method proposed in this work is asymptotically stable.

##### 5.1. Propositions

Proposition 1. *If is a three-dimensional symmetric matrix and is a three-dimensional antisymmetric matrix, the following equation holds:
*

*Proof. *Suppose that the general form of the symmetric matrix and the antisymmetric matrix is as follows:
Then,
Therefore, the trace of the matrix can be obtained from (41) as follows:
This proves that is true.

Proposition 2. *Trace of the three-dimensional antisymmetric matrix has the following relationship:
*

*Proof. *Suppose that the general form of the matrix is
Then, the matrix is obtained as follows:
It can be seen from (44) that
Similarly, we can get
The proof is completed.

Proposition 3. *The trace of the three-dimensional antisymmetric matrix has the following relationship with the two-norm of the matrix:
where is the two-norm of the matrix.*

*Proof. *Based on Proposition 2, one has that . The two-norm of the matrix can be calculated as follows:
This proves Proposition 3.

##### 5.2. Stability Analysis

The purpose of our work is to construct the estimate of , which satisfies the condition that converges asymptotically to , i.e., . Note that this condition implies that the estimated measurement vector converges asymptotically to the actual measurement vector , i.e., .

The error of the state of the Special Orthogonal Group is defined as , whereas the time derivative of the error is . The second theorem of Lyapunov’s stability analysis is used to prove the asymptotic stability of (39). The Lyapunov candidate function is constructed as follows:

This function has the following two properties:

*Property 4. *If , then . If and if , then .

*Proof. *When , we take it as the Lyapunov candidate function:
where represents the two-norm of the vector . is a unit vector, . Thus, when , .

If and if , then
Thus, is true.

*Property 5. * holds true for , and for any initial nonzero , is not always equal to zero.

*Proof. *The time derivative of the Lyapunov candidate function is given by
Substituting (39) into (52), we get
Using (30), Equation (52) can be written as follows:
Since is an antisymmetric matrix, is a symmetric matrix. Thus, from Proposition 2 we know that
Substituting the innovation item in (39) into (55), we get
From Proposition 2, Equation (55) can be written as
According to Proposition 3, the expression is simplified as follows:
where is a matrix which is not always equal to zero.

To sum up, the selected Lyapunov candidate function is a positive definite and has a unique equilibrium point. Further, the derivative of the Lyapunov candidate function is a negative semidefinite, and thus, the system is asymptotically stable.

#### 6. Simulation and Experiment

This section investigates the performance of the Special Orthogonal Group Optimization (SOGO) method proposed in this work by using numerical and experiment simulations. For comparisons, four currently popular coarse alignment methods, namely, TRIAD, QUEST, AISOG, and FLAE, have been compared with SOGO in terms of alignment accuracy and time. In order to guarantee the fairness in the comparison, five synchronous threads were designed in the simulation and experiment to run the five alignment methods simultaneously.

##### 6.1. Simulation Results and Analysis

The simulation was designed to verify the feasibility of the proposed method under swaying condition. In the simulation, the carrier was set to be affected by external disturbances, which was used to simulate the carrier on the rocking base. The true attitude angle changes in three directions were set to the following cosine changes:
where , , and represent yaw, pitch, and roll, respectively. The equatorial radius is 6378165.0 m, the gravity acceleration is 9.7849 m/s^{2}, and the angular velocity of the Earth is 7.292158^{-5} rad/s. The initial position was taken as 118°E and 40°N. The constant drift and the random drift of the accelerometer in three directions were 1 mg and 0.1 mg, respectively. The constant drift and the random drift of the gyro in the three directions were 0.1°/h and 0.01°/h, respectively. The attitude update period was set to s and the alignment lasted for 200 s. The accuracy of the various methods was obtained by comparing the error between the true attitude angle and the attitude angle estimated by the different methods.

The TRIAD method selected the middle moment and the last moment, respectively, and 10 groups of data were selected to obtain the attitude, respectively. The obtained results under different methods are given in Table 3. For the other four methods, the attitude angle errors obtained in the three directions are plotted in Figure 1. In order to see the yaw error more clearly, the logarithmic scale was adopted. The statistical mean value and variance in the last 100 s and the alignment time are also listed in Table 3.

**(a) Yaw error**

**(b) Pitch error**

**(c) Roll error**

As can be seen from Table 3, the TRIAD method determines the attitude based on two vectors that are sufficiently collinear, which results in a longer alignment time. Besides, because the finite set of data can be used to solve the attitude and then get the mean, the convergence accuracy is poor compared to other methods. It can be seen from Figure 1 that the convergence time of the method based on SOGO is faster than that based on quaternion. The yaw angle error of AISOG converges to 8.4 around 80 s, whereas the yaw angle error of SOGO converges to 4.2 around 40 s. Because the SOGO method presented in this paper propagates along the direction of the attitude error gradient, it can converge faster.

From Table 3, it is clearly observed that the alignment time of the SOGO method is the shortest. In addition, the steady-state errors of the different methods are also shown in Table 3. The yaw, pitch, and roll obtained by applying SOGO converge to 4.23, 0.40, and 0.48, respectively. Compared with the mainstream AIF, the SOGO improves alignment accuracy by 26% and alignment speed by nearly 23%. It is obvious that the SOGO method has the smallest mean attitude angle error and the smallest variance among all the methods, which indicates that the SOGO method has a higher convergence accuracy and fairly good stability.

##### 6.2. Experimental Results and Analysis

In order to verify the effectiveness and the application value of the SOGO method, we conducted a contrastive experiment on a platform that was placed on the surface of the water. In order to further simulate a rocking base, we shook the platform artificially to change the attitude, including that we make waves to make the ship sway and drag both ends of the platform to change the head azimuth. The XW-ADU7612 attitude azimuth integrated navigation system was used for obtaining the reference attitude in this experiment. The XW-ADU7612 module consists of dual GPS and inertial measurement unit, allowing it to provide precise carrier attitude. By comparing the error of the attitude resulting from the different methods and the reference attitude value obtained using the XW-ADU7612 module, we can easily know the difference among various methods. The parameters representing the system accuracy of the XW-ADU7612 attitude azimuth integrated navigation system are given in Table 4. The angular velocity and acceleration of the platform in all three directions were measured using Crossbow VG700AB. The parameters representing the accuracy of Crossbow VG700AB are listed in Table 5. The experimental setup is shown in Figure 2. The reference attitude used in the experiment, measured by the XW-ADU7612 module, is shown in Figure 3.

**(a) Change in the yaw**

**(b) Change in the pitch**

**(c) Change in the roll**

The TRIAD method used 10 sets of data and the results obtained are given in Table 6. The experimental results obtained using other four methods are also given in this table and are plotted in Figure 4. The logarithmic scale was used to observe the variation in the yaw, pitch, and roll for the sake of more clarity. However, this makes it impossible to display the instances having a negative attitude error. Table 6 also lists the statistical mean value and variance in the last 100 s as well as the alignment times.

**(a) Yaw error**

**(b) Pitch error**

**(c) Roll error**

From Table 6, it can be seen that SOGO, AISOG, FLAE, and QUEST spend less alignment time than the TRIAD method. Since the degree of collinearity between two vectors is required to be large enough to obtain an accurate attitude, the alignment time of the TRIAD method is the longest compared to other methods. In addition, the TRIAD method only uses the measurement vectors at two moments to determine the attitude, making a large amount of measurement information unavailable. As a result, the alignment accuracy is the worst among these methods. From Table 6 and Figure 4, the yaw angle error of the SOGO method converges to 24.9 in about 70 s. The yaw angle error of the AISOG method converges to 43.5 in about 90 s. Moreover, the convergence time of the pitch angle and the roll angle calculated using the SOGO method is obviously shorter than other methods, which indicates that the convergence time can be effectively improved by propagating along the direction of the gradient of the attitude error function proposed in this paper. From the results of Figure 4 and Table 6, it can be seen that the yaw, pitch, and roll obtained from the SOGO method converge to 24.93, 4.01, and 3.28, respectively. Thus, it can be concluded that the SOGO method has the best alignment accuracy among the methods tested here. The yaw angle error of the SOGO method is especially significantly better than those of the FLAE and AISOG methods. Compared with the mainstream AIF, the alignment accuracy of SOGO in three axes is improved by 43%, 43%, and 49%, respectively, and alignment speed by nearly 17%.

In summary, simulation and experimental results demonstrate that the proposed SOGO method can achieve high alignment accuracy in a short alignment time. Therefore, this method is fairly suitable for coarse alignment on a rocking base.

#### 7. Conclusion

The present work proposes a novel coarse alignment method using the SOGO for SINS on a rocking base. Firstly, the Special Orthogonal Group has been directly used for representing the attitude and establishing the coarse alignment model on the basis of the Special Orthogonal Group differential equation. Secondly, in order to improve the alignment accuracy and time, the optimal estimation method has been investigated to estimate the initial attitude by using the gradient of a new attitude error function which was adopted as the innovation term to compensate the attitude in real time. Finally, the stability of the proposed method has been proved by employing the Lyapunov stability theory.

Simulation and experimental results show that compared to the existing methods, the proposed coarse alignment method based on SOGO exhibits a great improvement in terms of alignment time and accuracy. Therefore, the coarse alignment method proposed in this paper has a good application prospect for the initial alignment of SINS on the rocking base.

In future work, we will apply the proposed method on the mobile robots and quadrotors control.

#### Data Availability

Some or all data, models, or code generated or used during the study are available from the corresponding author by request.

#### Conflicts of Interest

The authors declare that there is no conflict of interests regarding the publication of this paper.

#### Acknowledgments

The authors gratefully acknowledge the financial support for this project from the National Natural Science Foundation of China under Grant U22A2064.