Initial Self-Alignment for Marine Rotary SINS Using Novel Adaptive Kalman Filter

The accurate initial attitude is essential to affect the navigation result of Rotary Strapdown Inertial Navigation System (SINS), which is usually calculated by initial alignment. But marine mooring Rotary SINS has to withstand dynamic disturbance, such as the interference angular velocities and accelerations caused by surge and sway. In order to overcome the limit of dynamic disturbance under the marine mooring condition, an alignment method using novel adaptive Kalman filter for marine mooring Rotary SINS is developed in this paper. This alignment method using the gravity in the inertial frame as a reference is discussed to deal with the lineal and angular disturbances. Secondly, the system error model for fine alignment in the inertial frame as a reference is established. Thirdly, PWCS and SVD are used to analyze the observability of the system error model for fine alignment. Finally, a novel adaptive Kalman filter withmeasurement residual to estimatemeasurement noise variance is designed.The simulation results demonstrate that the proposed method can achieve better accuracy and stability for marine Rotary SINS.


Introduction
Because SINS has special advantages, it has been widely used in aviation, marine, and land vehicle navigation and positioning.Because the bias errors of inertial sensors can cause large positioning errors accumulated over time, low accuracy ship-carrier SINS cannot meet the navigation requirement for long voyage of the marine [1].Whereas the high accuracy SINS needs strategic inertial sensors, it will increase the cost correspondingly.With the emergence of rotation modulation technique that compensates the constant bias error of inertial sensors, Rotary SINS plays an important role in the marine field.But because the initial errors in the derived navigation parameters cannot be depressed by rotary modulation technique, initial alignment is the key technique for Rotary SINS.The purpose of initial alignment is to obtain an initial attitude matrix and set the misalignments to zero.Then the accuracy and stability of initial alignment are critical for high performance Rotary SINS [2,3].The initial attitude angle can be obtained by initial alignment, which is divided into two procedures: coarse and fine alignment.The coarse alignment is used to resolve large misalignment angle rapidly, and then the fine alignment is used to compensate and correct the misalignment angle further.
A lot of literature has been devoted to coarse alignment methods.But marine Rotary SINS has to withstand dynamic disturbance, such as the interference angular velocities and accelerations, caused by surge and sway far outweighing the autorotation angular velocity of the Earth.So the conventional coarse alignment method cannot be utilized [4].In 2000, the IxSea Company claimed a new alignment algorithm, which could be done in 5 min under any swinging conditions with their new alignment algorithm [5,6].Based on the alignment idea of Octans, relevant investigations about the inertial frame alignment have been carried out [7][8][9].The inertial frame alignment algorithm can isolate the disturbance by lineal and angular movements of the vehicle in the marine.But in these coarse alignment algorithms, the head angle error within a few degrees and pitch/roll angle within a few tenths of a degree hence fulfill the requirement for the fine alignment [10].
In the fine alignment procedure, Kalman filter is an optimal linear estimator when measurement noise has a Gaussian distribution with known measurement and process noise variance values.In [11], the fine alignment error model in the inertial frame was developed, the velocity differences between the calculated Strapdown INS values and the true values along the inertial frame were considered as measurements, and Kalman filter was used to estimate the error angle.But in this method, the disturbed acceleration was ignored based on the integration of the disturbed acceleration which was considered equal to zero.But the disturbed acceleration caused by heavy surge or waves in marine did not periodically vary, and determining an accurate value of this disturbance noise variance is very difficult in the marine environment.To overcome the uncertainty measurement noise in Strapdown INS fine alignment procedure, several adaptive filter techniques have been developed for fine alignment.Most of the work reported for this problem has concentrated on innovationbased adaptive estimation, which utilizes new statistical information from the innovation sequence to correct the estimation of the states.The main idea of these adaptive estimation methods is adjusting the  matrix online using fuzzy logic and neurofuzzy logic [12][13][14], for linear and nonlinear systems, respectively.This adaptive filter is based on a covariance matching technique, where its input is the difference between the actual Kalman filter residual covariance and its theoretical value.But the decision of the fuzzy rule and the selection of the adjusting step size of  are dependent on experience more or less, and the complex adjustment algorithm degrades the real-time performance.In [15], an adaptive Kalman filter was specially presented to estimate measurement noise variance directly using the measurement residual and employing the Frobenius norm.And the adaptive Kalman filter in [15] has successfully improved the stability of filter in the case of measurement uncertainties.
Inspired by the adaptive idea in [15], a novel adaptive Kalman filter algorithm is developed in this paper, and simulation test verifies the effectiveness of the proposed method.Similar to [11,14], utilizing the gravity in the inertial frame as a reference, the initial fine alignment error model is established for fine alignment procedure of Rotary SINS.The difference is that the PWCS theory and SVD theory are used to analyze the observability of the fine alignment error model before designing filter.And the novel calculation method for the adaptive scalar value is obtained and proved based on the system stability and optimality conditions of the adaptive filter.
This paper is organized as follows.The algorithm approaches for the principle of rotary modulation technique and the coarse alignment algorithm in the inertial frame are presented in Section 2. Section 3 provides the Rotary SINS fine alignment error model in the inertial frame and the observability analysis of PWCS theory and SVD theory.An adaptive filter is developed which is presented in Section 4. The simulation results are illustrated in Section 5. Finally, the conclusion is presented in Section 6.

Coordinate Frame Definitions.
The  frame is the body coordinate frame.The   axis is parallel to the body lateral axis and points to the right.The   axis is parallel to the body longitudinal axis and points forward.The   axis is parallel to the body vertical axis and points upward.
The  frame is the navigation coordinate frame which is the local level coordinate frame.The   axis points to the East, the   axis points to the North, and the   axis points to the zenith.
The  frame is the Earth-fixed coordinate frame.The   axis is parallel to the Earth's rotation axis and the   axis is in the equatorial plane and points to the meridian of the body initial position.The   axis completes the right-handed coordinate system.
The  frame is the inertial coordinate frame.It is formed by fixing the  frame at the beginning of the coarse alignment in the inertial space.
The   frame is the computed inertial coordinate frame.
The  0 frame is the body inertial coordinate frame.It is formed by fixing the  frame at the beginning of the alignment in the inertial space.
The  frame is the inertial measurement unit (IMU) coordinate frame.The origin of this frame is at the IMU's center of gravity and it is in accordance with the  frame at first.Besides,   runs parallel to the longitudinal gyro's sensing axis and the longitudinal accelerometer's sensing axis.Similarly,   runs parallel to the transverse gyro's sensing axis and the transverse accelerometer's sensing axis, whereas   is vertical to     and it is upright.Then the IMU rotates around the rotation axis with angular velocity .Thus, the  frame varied with the changes of IMU's position is a real-time variable frame.

Realization of Coarse Alignment for
Rotary SINS in the Inertial Frame 2.1.The Principle of Rotary Modulation Technique.In this section, the principle of restraining navigation errors will be explained in detail.First of all, the differential equations of attitude error and velocity error for Rotary SINS are expressed as First of all, the notations used in the equations should be explained.Subscript denotes a frame's relative movement to another frame, and superscript denotes the expression in the frame.For example,   is the attitude error expressed in the  frame and    is the angular rate of the  frame with respect to the  frame, expressed in the  frame.C   is the transform matrix from the  frame to the  frame and C   is the transform matrix from the  frame to the  frame.V  denotes the vector error.In particular,    and    are the errors caused by the inertial sensors inaccurate measurement.V  is the velocity expressed in the  frame and   is the gravity vector in the  frame.approximately approach zero.As a result, the navigation system errors are reduced and the system performance is improved owing to the rotational motion.Adopting dual-axis rotation modulating method in this paper, three gyros and three accelerometers are used as the inertial sensors.Initially, the  frame is in accordance with the  frame.Then the system rotates continuously at the uniform rotational angular velocities  1 and  2 ( 1 ̸ =  2 ) around the   and   axis of the  frame, respectively.We use the transform matrix C   to express the relations from the  frame to the  frame: Since the outputs of the inertial sensors are measured from the  frame, the following equations can be obtained: Among them, it is assumed that the gyroscope drift and accelerometer bias are   ,   , and   and ∇  , ∇  , and ∇  .In addition, the angular rate of the  frame with respect to the  frame, expressed in the  frame.   is the specific force of the  frame with respect to the  frame, expressed in the  frame.   = 0 is the specific force of the  frame with respect to the  frame, expressed in the  frame.f  is the measured output of accelerometer.ω  is the measured output of gyro.It is necessary to transform the inertial sensor measurements from the  frame to the  frame, so we have where    is the angular rate of the  frame with respect to the  frame, expressed in the  frame.ω  and    denote the actual and measured value of the gyro output, respectively.Substituting ( 3) and ( 4) using ( 5) and ( 6), respectively, the drift-bias errors of gyros and accelerometers by the rotational motion are Above all, we can find that the drift-bias errors of gyros and accelerometers on three axes in the  frame have been changed periodically.Consequently, it is confirmed that the dual-axis modulating rotation method is useful in restraining the drift-bias errors of the inertial sensor on three axes.

The Coarse Alignment Algorithm in the Inertial Frame.
The coarse alignment for Rotary SINS is to determine the coordinate transform matrix from the  frame to the  frame, which is also called the attitude matrix and could be described as follows: where where  is latitude,   is the Earth rotation rate, and  0 is the start time of the coarse alignment.C  0  () represents the transform matrix from the  frame to the  0 frame, which can be updated by using the gyro output in the  frame through the attitude quaternion algorithm (its initial value is the unit matrix).
Thus, the key of the coarse alignment based on the inertial frame is to compute the transform matrix C   0 .C   0 represents the transform matrix from the  0 frame to the  frame and is a constant matrix.According to dual-vector attitude determination method, C   0 can be calculated by using the integration of the gravity vector in the  frame and the  0 frame at two different moments.
The accelerometer outputs in the  frame consist of the projection of the gravity vector   , the projection of the interference acceleration    , the induced acceleration due to the lever-arm   LA , and the constant bias error of the accelerometer ∇, namely, As the constant bias error of the accelerometer ∇ is reduced to zero after every rotation period, the integration of the accelerometer's measured specific force projected in the  0 frame can be described as follows: As the integration of the    approaches zero approximately after every rotation period, (12) becomes where   is the gravity vector in the  frame.Integrating   from  0 to   yields Δ  =   − 0 , and V  (  ) could be computed as Substituting ( 15) in ( 14), the following equations can be obtained: where  2 is the end time of coarse alignment and  0 <  1 <  2 .From ( 16), C   0 could be calculated by the following equation: After deriving C   0 , the attitude matrix C   can be calculated through (8).

Fine Alignment Error Model and Observability Analysis
The coarse initial attitude angle can be obtained by the coarse alignment method in Section 2. The marine heading angle error is estimated to within a few degrees and pitch/roll angle to within a few tenths of a degree, which allow the fine alignment filter to operate within it in the linear region.In this section, the fine alignment error model of Rotary SINS in inertial frame is developed.Moreover, observability analysis of the error model is discussed using PWCS and SVD theory before designing the filter.

Fine Alignment Error
Model.Using (8) for reference, the attitude matrix C   () which remains to be calculated with high accuracy at the end of the fine alignment procedure can be represented as where C   and C   () are derived from ( 9) and ( 10), respectively.C   () can be described as follows: where C    () represents the transform matrix from the  frame to the computed inertial frame   and is given by where the attitude matrix C    (0) relates the  frame to the   frame at the beginning of the fine alignment, which is derived from the coarse alignment.Through the attitude quaternion updating algorithm, the matrix ΔC   can be computed recursively with the gyro and accelerometer outputs.Furthermore, there exist misalignment angles   between the inertial frame  and the computed inertial frame   owing to component errors; the transition matrix C    can be constructed as follows: By substituting (20) in (19), the transform matrix C   () can be determined.So the main problem of the fine alignment becomes the estimation of misalignment angles   .System error equations should be established firstly, which include velocity and attitude error equations.The measured specific force projected from accelerometer in the   frame can be written as where   is the interference acceleration in the  frame and accelerometer bias ∇ includes constant drift ∇  and Gaussian white noise ∇  , namely, ∇ = ∇  + ∇  .So ( 22) can be written as where , and Measurement vectors could be described as follows: In interference angular velocities and accelerations environment, the velocity error equation in the inertial frame can be defined as In accordance with the differential equations of C   and C    , the misalignment angle equation in the inertial frame can be defined as where   and   are gyro constant drift and Gaussian white noise.
With gyro drift and accelerometer bias added to state vectors, the state equation in the dynamic disturbance environment is described as follows: The state vectors and system noise in (29) are where six vectors in () are the noise of accelerometers and gyros in the  frame, which is normally distributed white noise with zero mean and () variance.The state matrix and system noise matrix of Rotary SINS in the inertial frame are as follows: Furthermore, velocity errors are selected as measurement vectors and the measurement equation is The measurement matrix in ( 32) is where V   is uncertainty measurement disturbance caused by the marine's rock and sway.V  is system measurement noise, which is white noise.

Observability Analysis Using PWCS and SVD.
For the designing of optimal filter, the observability of the system model is an important issue and requires careful investigation, as it is crucial for the stability and convergence of the filter.In this section, the observability analysis of the fine alignment model for Rotary SINS is discussed using two methods, which are piecewise constant system (PWCS) [16,17] method and singular value decomposition (SVD) [18,19].First, the PWCS analysis method is used to discover the number of the observable state value in MATLAB.Second, the SVD method is used to calculate the observable degree of every state value, because it is an efficient method to analyze the observable degree of every state value for completely observable and the incompletely observable system.
Assume that the matrix   is the observability matrix of a dynamic system; it can be expressed by where  = [  where  0 is the initial state vector and  is measurement vector.By substituting (34) into (35), we can obtain According to (37), initial state vector  0 describes an ellipsoid when measurement vector  is constant norm, and the equation is given by where   = 1/  represents the principal axis length of the ellipsoid.Obviously, the volume of the ellipsoid is determined by singular value.The volume reduces as well as the state vector  0 when the singular value rises up.Therefore, singular value represents the ellipsoid radius formed by the estimated initial state.The ellipsoid radius becomes smaller as the singular value becomes larger, and the estimation performance of the initial state is better.
For Rotary SINS, we use dual-axis rotation scheme presented above; the initial state vector corresponding to 12 singular values is found by (34)-(38), and the rotating angular velocity is  1 = 3 2 = 18 ∘ /s.Table 1 shows the comparison of observable degree for Rotary SINS in interference angular velocities and accelerations environment during three successive time periods.
Table 1 demonstrates that the system is completely observable and every state value can be estimated with the SVD theory.Particularly,   ,   ,   ,   , and   which have a larger observable degree can reach a better alignment effect.The observable degree of   ,   , and   is almost 1.Compared with the other eleven states, the observable degree of   is a little smaller.But it is large enough to be observable.On the basis of observability theory, the filter has been designed to estimate the system state parameters.

Novel Adaptive Kalman Filter Designed for Fine Alignment
In the fine alignment procedure, Kalman filter is an optimal linear estimator when the measurement noise has a Gaussian distribution with known covariance.However, when Rotary SINS is under the large dynamic disturbance environment, the conventional Kalman filter becomes not an optimal estimator, which may tend to diverge.In this section, a novel adaptive Kalman filter for fine alignment that utilizes the measurement residual to estimate measurement noise variance in real time is investigated.The novel adaptive filter is developed according to the optimality condition of the adaptive Kalman filter presented in [20,21].
The system error model which we discussed above can be considered as the following state-space system: The measurement residual is given by By substituting (40) in ( 41), the measurement residual can be written as where  is a real number,  ≥ 0,  1 is the measurement noise with a fixed variance, and  2 is the time-varying measurement noise.It is assumed that the two types of measurement noises are uncorrelated.(  , X,−1 ) is the higher order item in the estimation error.If this item is ignored, (42) can be simplified to The covariance of the measurement residual is expressed as where  =  2 is scalar value that accommodates the filter's stability and optimality, ].The covariance of state estimation error is defined as Substituting (45) into (44) results in where Thus we can get the optimal gain of the adaptive Kalman filter as follows: To obtain the optimal gain, in [15], the cost equation is defined in terms of minimizing the Frobenius norm to calculate  value in real time.In this paper, we employ the optimality condition of the adaptive Kalman filter to obtain the optimal gain.The following equation shows the optimality condition, which means that the sequence of measurement residuals is uncorrelated: The autocovariance of the measurement residuals is In reality, the real covariance of the measurement residuals is different from a theoretical one owing to model errors or unknown external disturbances.Thus,   () may not be identical to zero.From (49), we know that if the last term of   () which is the only common item of   () for all  = 1, 2, 3, . . . is zero, then the gain   is optimal.In other words, if the gain   is optimal, (50) holds.Equation (50) forms the basis for determining scalar value .In particular,  0 () in (50) which is computed from measured data in real time, according to  successive residuals, can be shown as The optimal scalar value  can be obtained by (52) when the system model satisfies Assumptions 1 and 2, which is the optimality condition of the adaptive Kalman filter presented in [20,21].Assumption 1. (), (), and (0) are all positive definite symmetrical matrix.
Proof.Substituting (47) into the optimality condition (50) gives Since  |−1 and  0 () are full-ranked symmetric matrix according to Assumptions 1 and 2 and (51), left multiplying  −1 |−1 and right multiplying  0 () −1 on both sides of (53), it is obvious that (53) implies the following relation: then Since   is full-ranked matrix according to Assumption 2, (55) is simplified as To avoid inversion manipulation, trace is directly taken in both sides of (56): then we can get the scalar value  in (52).Proof ends.
As shown in [20,21], instability issues may arise when no upper bound is set for the scalar value.Thus the upper and lower bound are defined for the measurement noise variance as [0  max ], and the rule is as follows: if  ≤ 0,  = 0 and if  ≥  max ,  =  max .The  max value can be defined by the user according to (44).
Substituting  into (46) during the initial alignment process, the proposed adaptive filter for the system state estimation is as follows: For (32) it consists of three measurements.A scalar value  is required for each measurement, as every measurement has unique characteristics.Similar to [15], this paper solves this problem using a scalar measurement updating / covariance factorization filter, where  is an upper triangular and unitary matrix and  is a diagonal matrix.It is also advantageous to utilize the / covariance factorization filter when designing and implementing the novel adaptive filter.

Simulation Results
The parameters of the simulation are set as follows.
The marine is rocked by the surf and wind.The pitch , roll , and heading  resulting from the marine rocking are changed periodically and can be described as follows:   The velocity caused by surge, sway, and heave is as follows: The IMU errors are set as follows: the gyro constant drift: 0.01 ∘ /h; the gyro random noise: 0.001 ∘ /h; the accelerator bias: 1 × 10 −4 g; and the accelerator measurement noise: 1 × 10 −5 g.Rotary SINS location: north latitude is 40 ∘ and east longitude is 118 ∘ .According to the rule shown in the previous section, double-axis rotation rate is given by  1 = 3 2 = 18 ∘ /s.Simulation 1.The coarse alignment lasts 120 s.The values of  1 and  2 in (17) are set to 50 s and 120 s, respectively.The simulation for the coarse alignment runs 50 times.The pitch, roll, and heading errors at the end of the coarse alignments are shown in Table 2.
From Table 2, it is obvious that the level attitude errors of the coarse alignment are less than 0.2 degrees and the heading attitude error is less than 1.5 degrees.The attitude errors calculated by the proposed coarse alignment algorithm can fulfill the requirement for the fine alignment.But it only accomplished the coarse estimation of attitude errors in initial alignment.Thus 600-second fine alignment is followed.
Next, the mean and the maximum of misalignment angles in Table 2 are used, respectively, as input for fine alignment to validate the proposed adaptive filter in initial alignment under dynamic disturbance condition.The mean of misalignment angles is used in Simulations 2 and 3, and the maximum of misalignment angles is used in Simulation 4.
Simulation 2. The simulation results are shown in Figure 1.It is clear that all the states are observable and the result confirms the former analysis.According to Figure 1(a), convergence time of level misalignment angle is within 60 s, and precision is 1  , while azimuth misalignment angle convergence time is within 80 s, and precision is about 1.3  .The three velocity errors as observed variables depicted in Figure 1(b) are certainly observable.As seen from Figure 1(c), gyro drifts converge not so rapidly, but a rather good filtering result can be obtained.Figure 1(d) shows that the horizontal accelerometers constant biases ∇ and ∇ converge to a constant value in short time.The vertical converges soon afterwards with good estimation performance.The simulation results   demonstrate that all the system state parameters can be estimated by the presented filter.It can be seen that the presented alignment method can effectively depress the random disturbances under dynamic environment.
Simulation 3. Considering measurement uncertainties under the dynamic disturbing conditions, Sage-Husa adaptive filter (SHAF) used in [22], adaptive Kalman filter (AKF) presented in [15], and the novel adaptive Kalman filter (NAKF) proposed in this paper are employed to evaluate the filter performance.The simulation results are shown in Figure 2 and Table 3.
The simulation results demonstrate that the performance and convergence speed of NAKF which can estimate the variance of the measurement noise in real time are better than those of AKF and SHAF.This implies that, in the case of dynamic disturbance, NAKF functions better than SHAF and AKF and provides the optimal estimation.Then Simulation 3 with the maximum initial figures is made.Simulation 4. The simulation results show that the proposed NAKF is efficient with large initial attitude error.The results are shown in Figure 3 and Table 4. Compared with SHAF and AKF, NAKF provides higher performance and better stability in estimating the misalignment angles when large errors exist in the measurement values.The result demonstrates that NAKF can effectively depress the random disturbances in measurements noise variance caused by the ship rocking.Taking into account the unknown outside disturbances and the complicated working environment, the proposed NAKF with the optimality of filter is applicable in implementing initial alignment of Rotary SINS.

Conclusions
Based on the alignment mechanism of tracing the gravitational apparent motion in inertial frame, an initial self-alignment method based on the novel adaptive Kalman filter for Rotary SINS is developed.Before the filter is designed, observability is analyzed using PWCS method and SVD method.The theoretical analysis results show that the fine alignment error model is completely observable and all the system state parameters can be estimated.For the uncertainties measurement noise, the novel adaptive Kalman filter that estimates measurement noise variance in real time using measurement residual is designed.Simulation results proved the accuracy and validity of the proposed method, and the estimation results of the system states can approach the theoretical analysis results.In addition, this method is fully self-aligned with no external reference information under dynamic disturbances condition.But in this paper, only simulation test is finished.More tests, such as turntable and shipboard test, should be studied before they can be widely used in engineering.

Figure 2 :
Figure 2: The results depending on the filter with mean misalignment angles.

Figure 3 :
Figure 3: The results depending on the filter with maximum misalignment angles.
is the Earth rotation rate vector in the  frame and    is the  frame angular rate in the  frame.

Table 1 :
The comparison of observable degree for Rotary SINS.

Table 2 :
Statistics for coarse alignment results.

Table 3 :
The mean misalignment angle errors depending on the filter.

Table 4 :
The maximum misalignment angle errors depending on the filter.