# Novel Special Orthogonal Group Optimization for Coarse Alignment Method of SINS on a Rocking Base

Authors: Fujun Pei; Siyuan Li; Li Peng; Shunan Yin
Journal: International Journal of Aerospace Engineering
 (2022)

---

## 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.


---

## Body

## 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 asSO3-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 SO3-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 SO3 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 SO3-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 theSO3-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 SO3 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: Section2 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.Table 1
Definitions of the reference frames.
Reference frameDefinitionn frameThe navigation frame (n frame), which is orthogonal reference frame aligned with east-north-up (E-N-U) geodetic axesb frameThe sensor’s body fixed framei frameThe inertial coordinate framen0 frameThe inertial coordinate frame obtained by fixing then frame at the initial timeb0 frameThe inertial coordinate frame obtained by fixing theb frame at the initial timeTable 2
Definitions of various symbols.
Reference frameDefinition×Skew-symmetric matrix operator∙∨Map from skew-symmetric matrix to a vectorTSampling time intervalRabAttitude matrix between thea frame and the b frameωAngular rate vectorI3×3 identity matrixfbSpecific forcegGravitational acceleration


## 2. Preliminaries

A Special Orthogonal Group is a smooth and differentiable manifold satisfying two conditions: group operations and reversibility [18]. The Special Orthogonal Group SO3 describes the group of 3D rotation matrices and is defined as
(1)SO3=RR∈R3×3,RRT=I,detR=1.Special Orthogonal Group forms a smooth manifold, and its tangent space at the origin is a3×3 skew-symmetric matrix, denoted as so3 [28],
(2)so3=SS∈R3×3,S=−ST.The operation∙× represents the mapping from a 3×1 vector ω∈R3 to the corresponding 3×3 skew-symmetric matrix S∈R3×3:
(3)S=ω×=0−ω3ω2ω0−ω1−ω2ω10,∀ω=r1r2r3∈R3.The inverse operation∙∨ represents the map from the skew-symmetric matrix S∈so3 to a vector ω∈R3.
(4)S∨=ω.The operationexp∙defines the exponential map from so3 to SO3 and conforms to the standard matrix exponential formula (the Rodrigues’ formula):
(5)expS=I+sinωωS+1−cosωω2S2.IfRt∈SO3, then ṘtRTt and RTtṘt are skew-symmetric matrices [28]. The Special Orthogonal Group differential equation can then be described as
(6)Ṙt=Rtω×,where ω×∈so3. For a given sampling time interval T, the discrete implementation of (6) is given as
(7)Rk+1=RkexpT∙ω×.The tangent space of theSO3 group at any point R is given by [29]
(8)TRSO3=RS∈R3×3R∈SO3,S∈so3.The inner product∙,∙ on so3 is defined as
(9)A,B=trATBforallA,B∈so3.The Riemannian metric∙,∙R on the SO3 group can be defined as [29]
(10)V,UR=VRT,URT,∀U,V∈TRSO3.The gradient of a functionf:SO3⟶ℝ is defined as the unique tangent vector ∇RfR∈TRSO3 such that [29]
(11)ddϵfΓϵϵ=0=∇RfR,SRR,∀S∈so3,where ϵ∈ℝ and Γϵ=expϵSR is a curve around R. d/dϵfΓϵϵ=0 is the tangent map of f atR.


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

Because of the nature and the advantages of theSO3 group, SO3 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:(12)Rntbt=Rb0btRn0b0Rntn0,where R∈SO3, Rntbt represents the attitude matrix of the n frame relative to the b frame, and Rn0b0 is the initial attitude group and represents the attitude change from n0 frame to b0 frame at the initial moment. Rbtb0 and Rntn0 are time-varying attitude groups in the b frame or n frame, respectively, from time t to 0 and can be calculated using the following equations:
(13)Ṙbtb0=Rbtb0ωibb×,Ṙntn0=Rntn0ωinn×,where ωibb is the angular velocity of the frame b relative to the frame i under the b frame and ωinn is the angular velocity of the frame n relative to the frame i under the n frame and can be calculated by the expression ωinn=ωien+ωenn, where ωien=0ωiecosLωiesinL is the angular velocity of the rotation of the Earth, L is the local latitude, and ωenn is the zero vector when the carrier is not moving.Using (5), Rntk−1ntk and Rbtk−1btk can be obtained as
(14)Rntk−1ntk=I+sinφnφnφn×+1−cosφnφn2φn×2,Rbtk−1btk=I+sinφbφbφb×+1−cosφbφb2φb×2,where φn=∫tk−1tkωinndτ=T∙ωinn, φb=∫tk−1tkωibbdτ=T∙ωibb, and T is the sampling time. φb is calculated using the two-sample iteration algorithm φb=Δθ1+Δθ2+2/3Δθ1×Δθ2, where Δθ1 and Δθ2 are the first and second samples of the gyro-measured incremental angle [30].BecauseRn0n0 and Rb0b0 are the attitude matrices at the initial moment, they can be chosen as unit matrix. Then, the expressions for Rn0ntk and Rb0btk in the iteration calculation are given as follows:
(15)Rn0ntk=Rntk−1ntkRn0ntk−1,Rb0btk=Rbtk−1btkRb0btk−1.SinceRn0nt and Rb0bt are calculated using (17) and (18), Rntbt can be obtained from (12) if the initial attitude group Rn0b0 can be determined. Then, the coarse alignment problem is transformed into the problem of estimating the initial attitude group Rn0b0. Because Rn0b0 does not change with time, the state equation based on the Special Orthogonal Group representation can be established as
(16)Ṙ=0.


### 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
(17)Vgb0t=∫0tgb0bdt=Rn0b0∫0tgn0ndt=Rn0b0Vgn0t.The measurement equation can then be written as(18)yt=Rxt,where
(19)yt=∫0tgb0bdt=∫0tRbtb0gbdt=−∫0tRbtb0fbdt,xt=∫0tgn0ndt=∫0tRntn0gndt.The coarse alignment model based on the Special Orthogonal Group representation can be established as(20)Ṙ=0,yt=Rxt.Because the state of this model is theSO3 group, the alignment problem is transformed into the problem of estimating the initial attitude group Rn0b0. 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.


## 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:(12)Rntbt=Rb0btRn0b0Rntn0,where R∈SO3, Rntbt represents the attitude matrix of the n frame relative to the b frame, and Rn0b0 is the initial attitude group and represents the attitude change from n0 frame to b0 frame at the initial moment. Rbtb0 and Rntn0 are time-varying attitude groups in the b frame or n frame, respectively, from time t to 0 and can be calculated using the following equations:
(13)Ṙbtb0=Rbtb0ωibb×,Ṙntn0=Rntn0ωinn×,where ωibb is the angular velocity of the frame b relative to the frame i under the b frame and ωinn is the angular velocity of the frame n relative to the frame i under the n frame and can be calculated by the expression ωinn=ωien+ωenn, where ωien=0ωiecosLωiesinL is the angular velocity of the rotation of the Earth, L is the local latitude, and ωenn is the zero vector when the carrier is not moving.Using (5), Rntk−1ntk and Rbtk−1btk can be obtained as
(14)Rntk−1ntk=I+sinφnφnφn×+1−cosφnφn2φn×2,Rbtk−1btk=I+sinφbφbφb×+1−cosφbφb2φb×2,where φn=∫tk−1tkωinndτ=T∙ωinn, φb=∫tk−1tkωibbdτ=T∙ωibb, and T is the sampling time. φb is calculated using the two-sample iteration algorithm φb=Δθ1+Δθ2+2/3Δθ1×Δθ2, where Δθ1 and Δθ2 are the first and second samples of the gyro-measured incremental angle [30].BecauseRn0n0 and Rb0b0 are the attitude matrices at the initial moment, they can be chosen as unit matrix. Then, the expressions for Rn0ntk and Rb0btk in the iteration calculation are given as follows:
(15)Rn0ntk=Rntk−1ntkRn0ntk−1,Rb0btk=Rbtk−1btkRb0btk−1.SinceRn0nt and Rb0bt are calculated using (17) and (18), Rntbt can be obtained from (12) if the initial attitude group Rn0b0 can be determined. Then, the coarse alignment problem is transformed into the problem of estimating the initial attitude group Rn0b0. Because Rn0b0 does not change with time, the state equation based on the Special Orthogonal Group representation can be established as
(16)Ṙ=0.


## 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
(17)Vgb0t=∫0tgb0bdt=Rn0b0∫0tgn0ndt=Rn0b0Vgn0t.The measurement equation can then be written as(18)yt=Rxt,where
(19)yt=∫0tgb0bdt=∫0tRbtb0gbdt=−∫0tRbtb0fbdt,xt=∫0tgn0ndt=∫0tRntn0gndt.The coarse alignment model based on the Special Orthogonal Group representation can be established as(20)Ṙ=0,yt=Rxt.Because the state of this model is theSO3 group, the alignment problem is transformed into the problem of estimating the initial attitude group Rn0b0. 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 Rn0b0, while the attitude error function of R is usually described as
(21)η=RTR^.Then, through the definition of the attitude error function, the estimated valueR^ could be adjusted repeatedly so that the attitude error approaches the identity matrix I. And R^ is considered to be the optimal estimate of R.Generally, the gradient descent method can quickly reduce the value of this attitude error function η to the minimum value I. However, since the unknown attitude truth value R is involved, the obtained gradient should be a function of the unknown value R, 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 R so that the gradient is solvable, and the gradient could be used to continuously adjust estimated value R^ 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 R can convert the vector xt in the n frame into the vector yt in the b frame. If the projection of the vector direction is considered individually, Equation (20) can be written as
(22)rt=Rut,where r=y/y, u=x/x, and thus, r and u are both unit vectors.In the estimation process, if the estimated attitudeR^ is equal to the real attitude R, the measurement vector rt and predictive vector r^t=R^ut should be equal. However, since there are errors between R^ and R, rt and r^t are not equal. Therefore, in order to accurately describe the error of the attitude estimation, the following attitude error function ER^,R has been constructed:
(23)ER^,R=1−cosr^,r=1−r∙r^rr^.Sincer and u are both unit vectors, Equation (23) can be written as
(24)ER^,R=1−r∙r^=1−r∧T∙r=1−trr∙r∧T.Using (25), (27) can be rewritten as
(25)ER^=1−trRuuTR∧T.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 δR^ of R^∈SO3 is
(26)δR^=ddϵexpϵSR^ϵ=0=SR^.For anyU∈R3×3, the following equation holds:
(27)PaU+PsU=U,where PaU=1/2U−UT and PsU=1/2U+UT. The operator Pa∙ is an antisymmetric projection operator and Ps∙ is a symmetric projection operator.From the definition ofER^ in (28), the tangent map of ER^ at R^ is given by [29]
(28)ddϵEΓϵϵ=0=−trRuuTδR∧T.Using (29), Equation (28) can be written as
(29)ddϵEΓϵϵ=0=trRuuTR∧TS=trr∙r∧TS.From (30), we have:
(30)ddϵEΓϵϵ=0=trPar∙r∧TS+Psr∙r∧TS=trPar∙r∧TS+trPsr∙r∧TS.The trace of the product of an arbitrary symmetric matrix and an antisymmetric matrix is equal to zero. Thus,(31)ddϵEΓϵϵ=0=trPar∙r∧TS=tre×S,where e×=Par∙r∧T.Equation (31) can be rewritten using (9) as
(32)ddϵEΓϵϵ=0=<e×,S>=<e×R^R∧T,SR^R∧T>.On the basis of (10), we can get
(33)ddϵEΓϵϵ=0=<e×R^,SR^>R^.From (11), the tangent mapping of ER^ has the following relationship with the gradient of ER^:
(34)ddϵEΓϵϵ=0=∇R^ER^,SR^R^.Since (36) and (37) represent the tangent mapping of ER^, it holds true that
(35)∇R^ER^=e×R^.where ∇R^ER^ is the gradient of the attitude error function.We choose∇R^ER^ 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:
(36)R^̇=−e×R^,e=Par∙r∧Tν.


## 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.
IfA is a three-dimensional symmetric matrix and B is a three-dimensional antisymmetric matrix, the following equation holds:
(37)trAB=0.Proof.
Suppose that the general form of the symmetric matrixA and the antisymmetric matrix B is as follows:
(38)A=a1a4a5a4a2a6a5a6a3,B=0b1b2−b10b3−b2−b30.
Then,(39)AB=a1a4a5a4a2a6a5a6a30b1b2−b10b3−b2−b30=−a4b1−a5b2a1b1−a5b3a1b2+a4b3−a2b1−a6b2a4b1−a6b3a4b2+a2b3−a6b1−a3b2a5b1−a3b3a5b2+a6b3.
Therefore, the trace of the matrixAB can be obtained from (41) as follows:
(40)trAB=−a4b1−a5b2+a4b1−a6b3+a5b2+a6b3=0.
This proves thattrAB=0 is true.Proposition 2.
Trace of the three-dimensional antisymmetric matrixA has the following relationship:
(41)trAA=−trATA.Proof.
Suppose that the general form of the matrixA is
(42)A=0a1a2−a10a3−a2−a30.
Then, the matrixATA is obtained as follows:
(43)ATA=0−a1−a2a10−a3a2a300a1a2−a10a3−a2−a30=a12+a22a2a3a1a3a2a3a12+a32a1a2a1a3a1a2a22+a32.
It can be seen from (44) that
(44)trATA=2a12+a22+a32.
Similarly, we can get(45)trAA=−2a12+a22+a32.
Theproof is completed.Proposition 3.
The trace of the three-dimensional antisymmetric matrixA has the following relationship with the two-norm of the matrix:
(46)trATA=A2,where ∙ is the two-norm of the matrix.Proof.
Based on Proposition2, one has that trATA=2a12+a22+a32. The two-norm of the matrix A can be calculated as follows:
(47)A=2a12+a22+a32.
This proves Proposition3.


### 5.2. Stability Analysis

The purpose of our work is to construct the estimateR^t of R, which satisfies the condition that R^t converges asymptotically to R, i.e., limt⟶∞R^t=R. Note that this condition implies that the estimated measurement vector r^t converges asymptotically to the actual measurement vector rt, i.e., limt⟶∞r^t=rt.The error of the state of the Special Orthogonal Group is defined asR~=R∧TR, whereas the time derivative of the error is R~̇=R∧ṪR. 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:
(48)ER~=1−cosr^,r=1−trRuuTR∧T=1−truuTR∧TR=1−truuTR~.This function has the following two properties:Property 4.
IfR~=I, then ER~=0. If R~∈SO3 and if R~≠I, then ER~>0.Proof.
WhenR~=I, we take it as the Lyapunov candidate function:
(49)ER~=1−truuTR~=1−truuT=1−u2,where u represents the two-norm of the vector u. u is a unit vector, u=1. Thus, when R~=I, ER~=0.
IfR~∈SO3 and if R~≠I, then
(50)R∧TR≠I⟺R≠R^⟺Ru≠R^u⟺r≠r^⟺cosr^,r<1.
Thus,ER~=1−cosr^,r>0 is true.Property 5.
ĖR~≤0 holds true for ∀t, and for any initial nonzero R~, ĖR~ is not always equal to zero.Proof.
The time derivative of the Lyapunov candidate function is given by(51)ĖR~=1−truuTR~̇=−truuTR∧̇TR.
Substituting (39) into (52), we get
(52)ĖR~=−truuTR∧Te×R=−trRuuTR∧Te×=−trr∙r∧Te×=tre×r∙r∧T.
Using (30), Equation (52) can be written as follows:
(53)ĖR~=tre×Par∙r∧T+e×Psr∙r∧T.
Sincee× is an antisymmetric matrix, Psr∙r∧T is a symmetric matrix. Thus, from Proposition 2 we know that
(54)ĖR~=tre×Par∙r∧T.
Substituting the innovation iteme in (39) into (55), we get
(55)ĖR~=trPar∙r∧TPar∙r∧T.
From Proposition2, Equation (55) can be written as
(56)ĖR~=−trPar∙r∧TTPar∙r∧T,
According to Proposition3, the expression is simplified as follows:
(57)Ė=−Par∙r∧T2≤0,where Pay∙y∧T=1/2y∙y∧T−y^∙yT 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.


## 5.1. Propositions

Proposition 1.
IfA is a three-dimensional symmetric matrix and B is a three-dimensional antisymmetric matrix, the following equation holds:
(37)trAB=0.Proof.
Suppose that the general form of the symmetric matrixA and the antisymmetric matrix B is as follows:
(38)A=a1a4a5a4a2a6a5a6a3,B=0b1b2−b10b3−b2−b30.
Then,(39)AB=a1a4a5a4a2a6a5a6a30b1b2−b10b3−b2−b30=−a4b1−a5b2a1b1−a5b3a1b2+a4b3−a2b1−a6b2a4b1−a6b3a4b2+a2b3−a6b1−a3b2a5b1−a3b3a5b2+a6b3.
Therefore, the trace of the matrixAB can be obtained from (41) as follows:
(40)trAB=−a4b1−a5b2+a4b1−a6b3+a5b2+a6b3=0.
This proves thattrAB=0 is true.Proposition 2.
Trace of the three-dimensional antisymmetric matrixA has the following relationship:
(41)trAA=−trATA.Proof.
Suppose that the general form of the matrixA is
(42)A=0a1a2−a10a3−a2−a30.
Then, the matrixATA is obtained as follows:
(43)ATA=0−a1−a2a10−a3a2a300a1a2−a10a3−a2−a30=a12+a22a2a3a1a3a2a3a12+a32a1a2a1a3a1a2a22+a32.
It can be seen from (44) that
(44)trATA=2a12+a22+a32.
Similarly, we can get(45)trAA=−2a12+a22+a32.
Theproof is completed.Proposition 3.
The trace of the three-dimensional antisymmetric matrixA has the following relationship with the two-norm of the matrix:
(46)trATA=A2,where ∙ is the two-norm of the matrix.Proof.
Based on Proposition2, one has that trATA=2a12+a22+a32. The two-norm of the matrix A can be calculated as follows:
(47)A=2a12+a22+a32.
This proves Proposition3.


## 5.2. Stability Analysis

The purpose of our work is to construct the estimateR^t of R, which satisfies the condition that R^t converges asymptotically to R, i.e., limt⟶∞R^t=R. Note that this condition implies that the estimated measurement vector r^t converges asymptotically to the actual measurement vector rt, i.e., limt⟶∞r^t=rt.The error of the state of the Special Orthogonal Group is defined asR~=R∧TR, whereas the time derivative of the error is R~̇=R∧ṪR. 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:
(48)ER~=1−cosr^,r=1−trRuuTR∧T=1−truuTR∧TR=1−truuTR~.This function has the following two properties:Property 4.
IfR~=I, then ER~=0. If R~∈SO3 and if R~≠I, then ER~>0.Proof.
WhenR~=I, we take it as the Lyapunov candidate function:
(49)ER~=1−truuTR~=1−truuT=1−u2,where u represents the two-norm of the vector u. u is a unit vector, u=1. Thus, when R~=I, ER~=0.
IfR~∈SO3 and if R~≠I, then
(50)R∧TR≠I⟺R≠R^⟺Ru≠R^u⟺r≠r^⟺cosr^,r<1.
Thus,ER~=1−cosr^,r>0 is true.Property 5.
ĖR~≤0 holds true for ∀t, and for any initial nonzero R~, ĖR~ is not always equal to zero.Proof.
The time derivative of the Lyapunov candidate function is given by(51)ĖR~=1−truuTR~̇=−truuTR∧̇TR.
Substituting (39) into (52), we get
(52)ĖR~=−truuTR∧Te×R=−trRuuTR∧Te×=−trr∙r∧Te×=tre×r∙r∧T.
Using (30), Equation (52) can be written as follows:
(53)ĖR~=tre×Par∙r∧T+e×Psr∙r∧T.
Sincee× is an antisymmetric matrix, Psr∙r∧T is a symmetric matrix. Thus, from Proposition 2 we know that
(54)ĖR~=tre×Par∙r∧T.
Substituting the innovation iteme in (39) into (55), we get
(55)ĖR~=trPar∙r∧TPar∙r∧T.
From Proposition2, Equation (55) can be written as
(56)ĖR~=−trPar∙r∧TTPar∙r∧T,
According to Proposition3, the expression is simplified as follows:
(57)Ė=−Par∙r∧T2≤0,where Pay∙y∧T=1/2y∙y∧T−y^∙yT 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:(58)Ψ=20°+4°cosπ6t+π4,θ=6°cosπ3t+π5,γ=8°cosπ9t+π4.where Ψ, θ, and γ represent yaw, pitch, and roll, respectively. The equatorial radius is 6378165.0 m, the gravity acceleration is 9.7849 m/s2, and the angular velocity of the Earth is 7.292158e-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 T=0.02 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 Table3. 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.Table 3
Comparison of the attitude angle error and the alignment time obtained using different coarse alignment methods.
TRIADQUESTFLAEAISOGSOGOYaw error (′)Mean value11.62478.41247. 87285.78454.2348Variance1.14210.87840.79250.59450.3893Pitch error (′)Mean value1.2157-0.61420.58520.54490.4016Variance0.24630.05240.05130.06120.0328Roll error (′)Mean value1.42450.8926-0.8345-0.71430.4863Variance0.46620.04240.04130.06150.0304Alignment time (s)11082766540Figure 1
The attitude angle errors of the four alignment methods.
(a)
Yaw error(b)
Pitch error(c)
Roll errorAs can be seen from Table3, 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 Table3, 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 Table4. 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.Table 4
System accuracy of the XW-ADU7612 module.
System accuracyHeading0.1° (baselinelength>2 m)Gyro zero bias stability0.5°/hAccelerometer zero bias stability≤1 mgTable 5
System accuracy of the Crossbow VG700AB module.
System accuracyIMUGyro0.5°/hAccelerometer≤1 mgData update rate100 HzFigure 2
Experimental setup.Figure 3
Variation in the attitude as a function of time, measured using the XW-ADU7612 module.
(a)
Change in the yaw(b)
Change in the pitch(c)
Change in the rollThe TRIAD method used 10 sets of data and the results obtained are given in Table6. 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.Table 6
Comparison of the attitude angle error and the alignment time.
TRIADQUESTFLAEAISOGSOGOYaw error (′)Mean value89.624755.112451. 872843.546524.9348Variance29.047111.278410.79259.69455.8937Pitch error (′)Mean value19.178711.314210.28527.82494.0164Variance4.14631.93141.75131.24350.6846Roll error (′)Mean value18.634710.14268.83456.43123.2863Variance3.46621.42541.24130.94950.4374Alignment time (s)126103989276Figure 4
The attitude angle errors of the four alignment methods.
(a)
Yaw error(b)
Pitch error(c)
Roll errorFrom Table6, 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.


## 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:(58)Ψ=20°+4°cosπ6t+π4,θ=6°cosπ3t+π5,γ=8°cosπ9t+π4.where Ψ, θ, and γ represent yaw, pitch, and roll, respectively. The equatorial radius is 6378165.0 m, the gravity acceleration is 9.7849 m/s2, and the angular velocity of the Earth is 7.292158e-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 T=0.02 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 Table3. 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.Table 3
Comparison of the attitude angle error and the alignment time obtained using different coarse alignment methods.
TRIADQUESTFLAEAISOGSOGOYaw error (′)Mean value11.62478.41247. 87285.78454.2348Variance1.14210.87840.79250.59450.3893Pitch error (′)Mean value1.2157-0.61420.58520.54490.4016Variance0.24630.05240.05130.06120.0328Roll error (′)Mean value1.42450.8926-0.8345-0.71430.4863Variance0.46620.04240.04130.06150.0304Alignment time (s)11082766540Figure 1
The attitude angle errors of the four alignment methods.
(a)
Yaw error(b)
Pitch error(c)
Roll errorAs can be seen from Table3, 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 Table3, 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 Table4. 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.Table 4
System accuracy of the XW-ADU7612 module.
System accuracyHeading0.1° (baselinelength>2 m)Gyro zero bias stability0.5°/hAccelerometer zero bias stability≤1 mgTable 5
System accuracy of the Crossbow VG700AB module.
System accuracyIMUGyro0.5°/hAccelerometer≤1 mgData update rate100 HzFigure 2
Experimental setup.Figure 3
Variation in the attitude as a function of time, measured using the XW-ADU7612 module.
(a)
Change in the yaw(b)
Change in the pitch(c)
Change in the rollThe TRIAD method used 10 sets of data and the results obtained are given in Table6. 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.Table 6
Comparison of the attitude angle error and the alignment time.
TRIADQUESTFLAEAISOGSOGOYaw error (′)Mean value89.624755.112451. 872843.546524.9348Variance29.047111.278410.79259.69455.8937Pitch error (′)Mean value19.178711.314210.28527.82494.0164Variance4.14631.93141.75131.24350.6846Roll error (′)Mean value18.634710.14268.83456.43123.2863Variance3.46621.42541.24130.94950.4374Alignment time (s)126103989276Figure 4
The attitude angle errors of the four alignment methods.
(a)
Yaw error(b)
Pitch error(c)
Roll errorFrom Table6, 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 GroupSO3 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 SO3 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.



---

*Source: 7627346-2022-11-21.xml*