Abstract
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 with measurement residual to estimate measurement noise variance is designed. The simulation results demonstrate that the proposed method can achieve better accuracy and stability for marine Rotary SINS.
1. 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–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 innovation-based 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–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 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.
2. 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 asFirst 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. is the transform matrix from the frame to the frame and is the transform matrix from the frame to the frame. denotes the vector error. In particular, and are the errors caused by the inertial sensors inaccurate measurement. is the velocity expressed in the frame and is the gravity vector in the frame. is the Earth rotation rate vector in the frame and is the frame angular rate in the frame.
In accordance with IMU rotation which continues, the element of is changed frequently. The fundamental aspect of rotation modulation technique is to change the element of periodically so that mean values of and 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 and () around the and axis of the frame, respectively. We use the transform matrix 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, is 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. is the specific force of the frame with respect to the frame, expressed in the frame. 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 havewhere 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.
2.2. 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:wherewhere is latitude, is the Earth rotation rate, and is the start time of the coarse alignment. represents the transform matrix from the frame to the 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 . represents the transform matrix from the frame to the frame and is a constant matrix. According to dual-vector attitude determination method, can be calculated by using the integration of the gravity vector in the frame and the 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 , 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 frame can be described as follows:As the integration of the approaches zero approximately after every rotation period, (12) becomeswhere is the gravity vector in the frame. Integrating from to yields , and could be computed as Substituting (15) in (14), the following equations can be obtained:where is the end time of coarse alignment and .
From (16), could be calculated by the following equation:
After deriving , the attitude matrix can be calculated through (8).
3. 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.
3.1. Fine Alignment Error Model
Using (8) for reference, the attitude matrix which remains to be calculated with high accuracy at the end of the fine alignment procedure can be represented aswhere and are derived from (9) and (10), respectively. can be described as follows:where represents the transform matrix from the frame to the computed inertial frame and is given bywhere the attitude matrix 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 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 can be constructed as follows:
By substituting (20) in (19), the transform matrix 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 aswhere is the interference acceleration in the frame and accelerometer bias includes constant drift and Gaussian white noise , namely, . So (22) can be written asIntegrating (23) from to yieldswhere , , , and . Consider
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 and , the misalignment angle equation in the inertial frame can be defined aswhere 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) arewhere 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) iswhere is uncertainty measurement disturbance caused by the marine’s rock and sway. is system measurement noise, which is white noise.
3.2. 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 bywhere and are both orthogonal matrixes. is a matrix with order, is a diagonal matrix, and are called the singular value of the matrix , .
The measurement matrix is given by where is the initial state vector and is measurement vector. By substituting (34) into (35), we can obtain
According to (37), initial state vector describes an ellipsoid when measurement vector is constant norm, and the equation is given bywhere 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 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 . 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.
4. 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 aswhere is a real number, , is the measurement noise with a fixed variance, and is the time-varying measurement noise. It is assumed that the two types of measurement noises are uncorrelated. 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 aswhere is scalar value that accommodates the filter’s stability and optimality, , and .
The covariance of state estimation error is defined asSubstituting (45) into (44) results inwhere and .
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 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, 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 are all positive definite symmetrical matrix.
Assumption 2. The measurement matrix is full ranked:where if , and if , .
Proof. Substituting (47) into the optimality condition (50) givesSince and are full-ranked symmetric matrix according to Assumptions 1 and 2 and (51), left multiplying and right multiplying on both sides of (53), it is obvious that (53) implies the following relation:thenSince is full-ranked matrix according to Assumption 2, (55) is simplified asTo 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 , and the rule is as follows: if , and if , . The 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.
5. 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:where , , , , , s, s, and s. obeys the uniform distribution on the interval .
The IMU errors are set as follows: the gyro constant drift: 0.0/h; the gyro random noise: 0.001°/h; the accelerator bias: g; and the accelerator measurement noise: g. Rotary SINS location: north latitude is and east longitude is . According to the rule shown in the previous section, double-axis rotation rate is given by .
Simulation 1. The coarse alignment lasts 120 s. The values of and 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 . 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.
(a) Estimation of attitude errors
(b) Estimation of velocity error
(c) Estimation of gyro drifts
(d) Estimation of accelerometer biases
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.
(a) Tilt misalignment angle
(b) Eastern misalignment angle
(c) Northern misalignment angle
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.
(a) Tilt misalignment angle
(b) Eastern misalignment angle
(c) Northern misalignment angle
6. 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.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgment
This work was supported by the Program for the Top Young Innovative Talents of Beijing CITTCD201304046.