Mathematical Problems in Engineering

Volume 2015 (2015), Article ID 320536, 12 pages

http://dx.doi.org/10.1155/2015/320536

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

^{1}School of Electronic Information & Control Engineering, Beijing University of Technology, Beijing 100124, China^{2}Beijing Key Laboratory of Computational Intelligence and Intelligent System, Beijing 100124, China

Received 24 January 2015; Accepted 15 April 2015

Academic Editor: Oscar Reinoso

Copyright © 2015 Fujun Pei et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

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