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

Authors: Fujun Pei; Li Zhu; Jian Zhao
Journal: Mathematical Problems in Engineering
 (2015)

---

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


---

## Body

## 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 R 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 R 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 Section2. 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 b frame is the body coordinate frame. The xb axis is parallel to the body lateral axis and points to the right. The yb axis is parallel to the body longitudinal axis and points forward. The zb axis is parallel to the body vertical axis and points upward.Then frame is the navigation coordinate frame which is the local level coordinate frame. The nx axis points to the East, the ny axis points to the North, and the nz axis points to the zenith.Thee frame is the Earth-fixed coordinate frame. The ez axis is parallel to the Earth’s rotation axis and the ex axis is in the equatorial plane and points to the meridian of the body initial position. The ey axis completes the right-handed coordinate system.Thei frame is the inertial coordinate frame. It is formed by fixing the e frame at the beginning of the coarse alignment in the inertial space.Thei′ frame is the computed inertial coordinate frame.Theib0 frame is the body inertial coordinate frame. It is formed by fixing the b frame at the beginning of the alignment in the inertial space.Thes 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 b frame at first. Besides, Oxs runs parallel to the longitudinal gyro’s sensing axis and the longitudinal accelerometer’s sensing axis. Similarly, Oys runs parallel to the transverse gyro’s sensing axis and the transverse accelerometer’s sensing axis, whereas Ozs is vertical to Oxsys and it is upright. Then the IMU rotates around the rotation axis with angular velocity ω. Thus, the s 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 as(1)ϕ˙n=-ωinn×ϕn+δωinn-CbnCsbδωiss,δv˙n=fn×ϕn+CbnCsbδfiss-2δωien+δωenn×vn-2ωien+ωenn×δvn+δgn.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, ϕn is the attitude error expressed in the n frame and ωinn is the angular rate of the n frame with respect to the i frame, expressed in the n frame. Csb is the transform matrix from the s frame to the b frame and Cbn is the transform matrix from the b frame to the n frame. δvn denotes the vector error. In particular, δωiss and δfiss are the errors caused by the inertial sensors inaccurate measurement. vn is the velocity expressed in the n frame and gn is the gravity vector in the n frame. ωien is the Earth rotation rate vector in the n frame and ωenn is the n frame angular rate in the e frame.In accordance with IMU rotation which continues, the element ofCsb is changed frequently. The fundamental aspect of rotation modulation technique is to change the element of Csb periodically so that mean values of CbnCsbδωiss and CbnCsbδfiss 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, thes frame is in accordance with the b frame. Then the system rotates continuously at the uniform rotational angular velocities ω1 and ω2 (ω1≠ω2) around the Oxb and Ozb axis of the b frame, respectively. We use the transform matrix Cbs to express the relations from the b frame to the s frame:(2)Cbs=CbsxCbsz.Since the outputs of the inertial sensors are measured from thes frame, the following equations can be obtained:(3)ωiss=Cbsω^ibb+ωbss+εxεyεzT,(4)fiss=Cbsf^ibb+fbss+∇x∇y∇zT.Among them, it is assumed that the gyroscope drift and accelerometer bias are εx, εy, and εz and ∇x,   ∇y, and ∇z. In addition, ωbss=-Cbsωsbb=ω1ω2sin⁡ω1tω2cos⁡⁡ω1tT is the angular rate of the s frame with respect to the b frame, expressed in the s frame. fiss is the specific force of the s frame with respect to the i frame, expressed in the s frame. fbss=0 is the specific force of the s frame with respect to the b frame, expressed in the s frame. f^ibb is the measured output of accelerometer. ω^ibb is the measured output of gyro.It is necessary to transform the inertial sensor measurements from thes frame to the b frame, so we have(5)ωibb=CbsTωiss+ωsbb,(6)fibb=CbsTfiss+fsbb,where ωibb is the angular rate of the b frame with respect to the i frame, expressed in the b frame. ω^ibb and ωibb 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(7)εxbεybεzb=εxcos⁡⁡ω2t-εycos⁡⁡ω1tsin⁡ω2t+εzsin⁡ω1tsin⁡ω2tεxsin⁡ω2t+εycos⁡⁡ω1tcos⁡⁡ω2t-εzsin⁡ω1tcos⁡⁡ω2tεysin⁡ω1t+εzcos⁡⁡ω1t,∇xb∇yb∇zb=∇xcos⁡⁡ω2t-∇ycos⁡⁡ω1tsin⁡ω2t+∇zsin⁡ω1tsin⁡ω2t∇xsin⁡ω2t+∇ycos⁡⁡ω1tcos⁡⁡ω2t-∇zsin⁡ω1tcos⁡⁡ω2t∇ysin⁡ω1t+∇zcos⁡⁡ω1t.Above all, we can find that the drift-bias errors of gyros and accelerometers on three axes in theb 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 thes frame to the n frame, which is also called the attitude matrix and could be described as follows:(8)Csnt=CenCietCib0iCsib0t,where(9)Cen=010-sin⁡L0cos⁡⁡Lcos⁡⁡L0sin⁡L,(10)Ciet=cos⁡⁡ωiet-t0sin⁡ωiet-t00-sin⁡ωiet-t0cos⁡⁡ωiet-t00001,where L is latitude, ωie is the Earth rotation rate, and t0 is the start time of the coarse alignment. Csib0(t) represents the transform matrix from the s frame to the ib0 frame, which can be updated by using the gyro output in the s 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 matrixCib0i. Cib0i represents the transform matrix from the ib0 frame to the i frame and is a constant matrix. According to dual-vector attitude determination method, Cib0ican be calculated by using the integration of the gravity vector in the i frame and the ib0 frame at two different moments.The accelerometer outputs in thes frame consist of the projection of the gravity vector gs, the projection of the interference acceleration δaDs, the induced acceleration due to the lever-arm aLAs, and the constant bias error of the accelerometer ∇, namely,(11)fs=-gs+δaDs+aLAs+∇.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 ib0 frame can be described as follows:(12)Vfib0tk=∫t0tkCbib0Csbfsdt=∫t0tkCbib0Csb-gs+δaDs+aLAsdt=-Ciib0∫t0tkgidt+δVDib0+VLAib0,(13)VLAib0=∫t0tkaLAib0dt=Csib0ωiss×rs.As the integration of the δaDs approaches zero approximately after every rotation period, (12) becomes(14)Vib0tk=Vfib0tk-VLAib0tk=-Ciib0∫t0tkgidt=Ciib0Vitk,where gi is the gravity vector in the i frame. Integrating gi from t0 to tk yields Δtk=tk-t0, and Vi(tk) could be computed as (15)Vitk=gcos⁡⁡Lωiesin⁡ωieΔtkgcos⁡⁡Lωie1-cos⁡⁡ωieΔtkΔtkgsin⁡L.Substituting (15) in (14), the following equations can be obtained:(16)Vib0tk1=Ciib0Vitk1,Vib0tk2=Ciib0Vitk2,where tk2 is the end time of coarse alignment and t0<tk1<tk2.From (16), Cib0icould be calculated by the following equation:(17)Cib0i=ViTtk1ViTtk2Vitk1×Vitk2T-1·Vib0Ttk1Vib0Ttk2Vib0tk1×Vib0tk2T.After derivingCib0i, the attitude matrix Csn can be calculated through (8).


## 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(1)ϕ˙n=-ωinn×ϕn+δωinn-CbnCsbδωiss,δv˙n=fn×ϕn+CbnCsbδfiss-2δωien+δωenn×vn-2ωien+ωenn×δvn+δgn.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, ϕn is the attitude error expressed in the n frame and ωinn is the angular rate of the n frame with respect to the i frame, expressed in the n frame. Csb is the transform matrix from the s frame to the b frame and Cbn is the transform matrix from the b frame to the n frame. δvn denotes the vector error. In particular, δωiss and δfiss are the errors caused by the inertial sensors inaccurate measurement. vn is the velocity expressed in the n frame and gn is the gravity vector in the n frame. ωien is the Earth rotation rate vector in the n frame and ωenn is the n frame angular rate in the e frame.In accordance with IMU rotation which continues, the element ofCsb is changed frequently. The fundamental aspect of rotation modulation technique is to change the element of Csb periodically so that mean values of CbnCsbδωiss and CbnCsbδfiss 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, thes frame is in accordance with the b frame. Then the system rotates continuously at the uniform rotational angular velocities ω1 and ω2 (ω1≠ω2) around the Oxb and Ozb axis of the b frame, respectively. We use the transform matrix Cbs to express the relations from the b frame to the s frame:(2)Cbs=CbsxCbsz.Since the outputs of the inertial sensors are measured from thes frame, the following equations can be obtained:(3)ωiss=Cbsω^ibb+ωbss+εxεyεzT,(4)fiss=Cbsf^ibb+fbss+∇x∇y∇zT.Among them, it is assumed that the gyroscope drift and accelerometer bias are εx, εy, and εz and ∇x,   ∇y, and ∇z. In addition, ωbss=-Cbsωsbb=ω1ω2sin⁡ω1tω2cos⁡⁡ω1tT is the angular rate of the s frame with respect to the b frame, expressed in the s frame. fiss is the specific force of the s frame with respect to the i frame, expressed in the s frame. fbss=0 is the specific force of the s frame with respect to the b frame, expressed in the s frame. f^ibb is the measured output of accelerometer. ω^ibb is the measured output of gyro.It is necessary to transform the inertial sensor measurements from thes frame to the b frame, so we have(5)ωibb=CbsTωiss+ωsbb,(6)fibb=CbsTfiss+fsbb,where ωibb is the angular rate of the b frame with respect to the i frame, expressed in the b frame. ω^ibb and ωibb 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(7)εxbεybεzb=εxcos⁡⁡ω2t-εycos⁡⁡ω1tsin⁡ω2t+εzsin⁡ω1tsin⁡ω2tεxsin⁡ω2t+εycos⁡⁡ω1tcos⁡⁡ω2t-εzsin⁡ω1tcos⁡⁡ω2tεysin⁡ω1t+εzcos⁡⁡ω1t,∇xb∇yb∇zb=∇xcos⁡⁡ω2t-∇ycos⁡⁡ω1tsin⁡ω2t+∇zsin⁡ω1tsin⁡ω2t∇xsin⁡ω2t+∇ycos⁡⁡ω1tcos⁡⁡ω2t-∇zsin⁡ω1tcos⁡⁡ω2t∇ysin⁡ω1t+∇zcos⁡⁡ω1t.Above all, we can find that the drift-bias errors of gyros and accelerometers on three axes in theb 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 thes frame to the n frame, which is also called the attitude matrix and could be described as follows:(8)Csnt=CenCietCib0iCsib0t,where(9)Cen=010-sin⁡L0cos⁡⁡Lcos⁡⁡L0sin⁡L,(10)Ciet=cos⁡⁡ωiet-t0sin⁡ωiet-t00-sin⁡ωiet-t0cos⁡⁡ωiet-t00001,where L is latitude, ωie is the Earth rotation rate, and t0 is the start time of the coarse alignment. Csib0(t) represents the transform matrix from the s frame to the ib0 frame, which can be updated by using the gyro output in the s 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 matrixCib0i. Cib0i represents the transform matrix from the ib0 frame to the i frame and is a constant matrix. According to dual-vector attitude determination method, Cib0ican be calculated by using the integration of the gravity vector in the i frame and the ib0 frame at two different moments.The accelerometer outputs in thes frame consist of the projection of the gravity vector gs, the projection of the interference acceleration δaDs, the induced acceleration due to the lever-arm aLAs, and the constant bias error of the accelerometer ∇, namely,(11)fs=-gs+δaDs+aLAs+∇.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 ib0 frame can be described as follows:(12)Vfib0tk=∫t0tkCbib0Csbfsdt=∫t0tkCbib0Csb-gs+δaDs+aLAsdt=-Ciib0∫t0tkgidt+δVDib0+VLAib0,(13)VLAib0=∫t0tkaLAib0dt=Csib0ωiss×rs.As the integration of the δaDs approaches zero approximately after every rotation period, (12) becomes(14)Vib0tk=Vfib0tk-VLAib0tk=-Ciib0∫t0tkgidt=Ciib0Vitk,where gi is the gravity vector in the i frame. Integrating gi from t0 to tk yields Δtk=tk-t0, and Vi(tk) could be computed as (15)Vitk=gcos⁡⁡Lωiesin⁡ωieΔtkgcos⁡⁡Lωie1-cos⁡⁡ωieΔtkΔtkgsin⁡L.Substituting (15) in (14), the following equations can be obtained:(16)Vib0tk1=Ciib0Vitk1,Vib0tk2=Ciib0Vitk2,where tk2 is the end time of coarse alignment and t0<tk1<tk2.From (16), Cib0icould be calculated by the following equation:(17)Cib0i=ViTtk1ViTtk2Vitk1×Vitk2T-1·Vib0Ttk1Vib0Ttk2Vib0tk1×Vib0tk2T.After derivingCib0i, the attitude matrix Csn 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 Section2. 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 Csnt which remains to be calculated with high accuracy at the end of the fine alignment procedure can be represented as(18)Csnt=CenCietCsit,where Cne and Ciet are derived from (9) and (10), respectively. Csit can be described as follows:(19)Csit=Cii′t-1Csi′t,where Csi′t represents the transform matrix from the s frame to the computed inertial frame i′ and is given by(20)Csi′t=Csi′0ΔCsi,where the attitude matrix Csi′0 relates the s frame to the i′ frame at the beginning of the fine alignment, which is derived from the coarse alignment. Through the attitude quaternion updating algorithm, the matrix ΔCsi can be computed recursively with the gyro and accelerometer outputs. Furthermore, there exist misalignment angles φi between the inertial frame i and the computed inertial frame i′ owing to component errors; the transition matrix Cii′can be constructed as follows:(21)Cii′=I-φi×=1-φziφyiφzi1-φxi-φyiφxi1.By substituting (20) in (19), the transform matrix Csi(t) can be determined. So the main problem of the fine alignment becomes the estimation of misalignment angles φi. System error equations should be established firstly, which include velocity and attitude error equations. The measured specific force projected from accelerometer in the i′ frame can be written as(22)Csi′f^s=Csi′I+δKAI+δA-gs+aLAs+aDs+∇≈-gi+φi×gi+Csi∇+Csi′aLAs+Csi′δas,where δas is the interference acceleration in the s frame and accelerometer bias ∇ includes constant drift ∇b and Gaussian white noise ∇w, namely, ∇=∇b+∇w. So (22) can be written as(23)Csi′f^s+gi-Csi′aLAs=φi×gi+Csi∇b+Csi∇w+δai.Integrating (23) from t1 to t yields(24)V~i=∫t1tCsi′f^sdτ+∫t1tgidτ-∫t1tCsi′aLAsdτ=∫t1tφi×gidτ+∫t1tCsi∇bdτ+∫t1tCsi∇wdτ+∫t1tδaidτ,where VLAi=∫t1tCsi′aLAsdτ=Csi′ωiss×rs, δVDi=∫t1tδaidτ, Vgi=∫t1tgidτ, and Vfi=∫t1tCsi′f^sdτ. Consider(25)δVi=∫t1tφi×gidτ+∫t1tCsi∇bdτ+∫t1tCsi∇wdτ.Measurement vectors could be described as follows:(26)Z=V~i=Vfi+Vgi-VLAi=δVi+Vw+δVDi.In interference angular velocities and accelerations environment, the velocity error equation in the inertial frame can be defined as(27)δV˙i=-gi×φi+Csi∇b+Csi∇w.In accordance with the differential equations ofCsi and Csi′, the misalignment angle equation in the inertial frame can be defined as(28)φ˙i=-Csiεb-Csiεw,where εb and εw 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:(29)X˙t=AtXt+BtW.The state vectors and system noise in (29) are(30)Xt=δVxi,δVyi,δVzi,φxi,φyi,φzi,εbx,εby,εbz,∇bx,∇by,∇bzTW=∇wx,∇wy,∇wz,εwx,εwy,εwz,0,0,0,0,0,0T,where six vectors in W(t) are the noise of accelerometers and gyros in the s frame, which is normally distributed white noise with zero mean and Q(t) variance. The state matrix and system noise matrix of Rotary SINS in the inertial frame are as follows:(31)A=03×3-git×03×3Csit03×6-Csit03×303×1203×12,B=Csit03×303×303×303×3-Csit03×603×1203×12.Furthermore, velocity errors are selected as measurement vectors and the measurement equation is(32)Zt=HXt+Vw+δVDi.The measurement matrix in (32) is(33)H=I3×303×9,where δVDi is uncertainty measurement disturbance caused by the marine’s rock and sway. Vw 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 matrixRk is the observability matrix of a dynamic system; it can be expressed by(34)Rk=UΣVT,where U=u1,u2,…,um and V=v1,v2,…,vm are both orthogonal matrixes. Σ=S0m-r×rT is a matrix with m×r order, S=diag⁡σ1,σ2,…,σr is a diagonal matrix, and σ1≥σ2≥⋯≥σr>0 are called the singular value of the matrix Rk, r=rank⁡A.The measurement matrix is given by(35)RkX0=Z,where X0 is the initial state vector and Z is measurement vector. By substituting (34) into (35), we can obtain(36)Z=∑i=1rσiviTX0ui(37)X0=UΣVT-1Z=∑i=1ruiTzσivi.According to (37), initial state vector X0 describes an ellipsoid when measurement vector Z is constant norm, and the equation is given by(38)∑i=1rviTX0uiai2=y2,where ai=1/σi 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 X0 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
The comparison of observable degree for Rotary SINS.
Time
δ
V
x
δ
V
y
δ
V
z
φ
x
φ
y
φ
z
ε
x
ε
y
ε
z
∇
x
∇
y
∇
z
1
1.0135
1.1031
1.0324
0.6251
13.8570
13.8636
13.7323
13.9108
2.2515
1.4198
1.4641
1.3946
2
1.1089
1.2246
1.1567
0.6785
13.8836
13.8975
13.7635
13.9438
2.4624
1.4856
1.5358
1.4298
3
1.2452
1.3244
1.2346
0.7037
13.9104
13.9044
13.8015
13.9704
2.6584
1.5203
1.5747
1.4742Table1 demonstrates that the system is completely observable and every state value can be estimated with the SVD theory. Particularly, εy, φz, φy, εx, and εz which have a larger observable degree can reach a better alignment effect. The observable degree of δVx, δVy, and δVz is almost 1. Compared with the other eleven states, the observable degree of φx 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.


## 3.1. Fine Alignment Error Model

Using (8) for reference, the attitude matrix Csnt which remains to be calculated with high accuracy at the end of the fine alignment procedure can be represented as(18)Csnt=CenCietCsit,where Cne and Ciet are derived from (9) and (10), respectively. Csit can be described as follows:(19)Csit=Cii′t-1Csi′t,where Csi′t represents the transform matrix from the s frame to the computed inertial frame i′ and is given by(20)Csi′t=Csi′0ΔCsi,where the attitude matrix Csi′0 relates the s frame to the i′ frame at the beginning of the fine alignment, which is derived from the coarse alignment. Through the attitude quaternion updating algorithm, the matrix ΔCsi can be computed recursively with the gyro and accelerometer outputs. Furthermore, there exist misalignment angles φi between the inertial frame i and the computed inertial frame i′ owing to component errors; the transition matrix Cii′can be constructed as follows:(21)Cii′=I-φi×=1-φziφyiφzi1-φxi-φyiφxi1.By substituting (20) in (19), the transform matrix Csi(t) can be determined. So the main problem of the fine alignment becomes the estimation of misalignment angles φi. System error equations should be established firstly, which include velocity and attitude error equations. The measured specific force projected from accelerometer in the i′ frame can be written as(22)Csi′f^s=Csi′I+δKAI+δA-gs+aLAs+aDs+∇≈-gi+φi×gi+Csi∇+Csi′aLAs+Csi′δas,where δas is the interference acceleration in the s frame and accelerometer bias ∇ includes constant drift ∇b and Gaussian white noise ∇w, namely, ∇=∇b+∇w. So (22) can be written as(23)Csi′f^s+gi-Csi′aLAs=φi×gi+Csi∇b+Csi∇w+δai.Integrating (23) from t1 to t yields(24)V~i=∫t1tCsi′f^sdτ+∫t1tgidτ-∫t1tCsi′aLAsdτ=∫t1tφi×gidτ+∫t1tCsi∇bdτ+∫t1tCsi∇wdτ+∫t1tδaidτ,where VLAi=∫t1tCsi′aLAsdτ=Csi′ωiss×rs, δVDi=∫t1tδaidτ, Vgi=∫t1tgidτ, and Vfi=∫t1tCsi′f^sdτ. Consider(25)δVi=∫t1tφi×gidτ+∫t1tCsi∇bdτ+∫t1tCsi∇wdτ.Measurement vectors could be described as follows:(26)Z=V~i=Vfi+Vgi-VLAi=δVi+Vw+δVDi.In interference angular velocities and accelerations environment, the velocity error equation in the inertial frame can be defined as(27)δV˙i=-gi×φi+Csi∇b+Csi∇w.In accordance with the differential equations ofCsi and Csi′, the misalignment angle equation in the inertial frame can be defined as(28)φ˙i=-Csiεb-Csiεw,where εb and εw 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:(29)X˙t=AtXt+BtW.The state vectors and system noise in (29) are(30)Xt=δVxi,δVyi,δVzi,φxi,φyi,φzi,εbx,εby,εbz,∇bx,∇by,∇bzTW=∇wx,∇wy,∇wz,εwx,εwy,εwz,0,0,0,0,0,0T,where six vectors in W(t) are the noise of accelerometers and gyros in the s frame, which is normally distributed white noise with zero mean and Q(t) variance. The state matrix and system noise matrix of Rotary SINS in the inertial frame are as follows:(31)A=03×3-git×03×3Csit03×6-Csit03×303×1203×12,B=Csit03×303×303×303×3-Csit03×603×1203×12.Furthermore, velocity errors are selected as measurement vectors and the measurement equation is(32)Zt=HXt+Vw+δVDi.The measurement matrix in (32) is(33)H=I3×303×9,where δVDi is uncertainty measurement disturbance caused by the marine’s rock and sway. Vw 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 matrixRk is the observability matrix of a dynamic system; it can be expressed by(34)Rk=UΣVT,where U=u1,u2,…,um and V=v1,v2,…,vm are both orthogonal matrixes. Σ=S0m-r×rT is a matrix with m×r order, S=diag⁡σ1,σ2,…,σr is a diagonal matrix, and σ1≥σ2≥⋯≥σr>0 are called the singular value of the matrix Rk, r=rank⁡A.The measurement matrix is given by(35)RkX0=Z,where X0 is the initial state vector and Z is measurement vector. By substituting (34) into (35), we can obtain(36)Z=∑i=1rσiviTX0ui(37)X0=UΣVT-1Z=∑i=1ruiTzσivi.According to (37), initial state vector X0 describes an ellipsoid when measurement vector Z is constant norm, and the equation is given by(38)∑i=1rviTX0uiai2=y2,where ai=1/σi 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 X0 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
The comparison of observable degree for Rotary SINS.
Time
δ
V
x
δ
V
y
δ
V
z
φ
x
φ
y
φ
z
ε
x
ε
y
ε
z
∇
x
∇
y
∇
z
1
1.0135
1.1031
1.0324
0.6251
13.8570
13.8636
13.7323
13.9108
2.2515
1.4198
1.4641
1.3946
2
1.1089
1.2246
1.1567
0.6785
13.8836
13.8975
13.7635
13.9438
2.4624
1.4856
1.5358
1.4298
3
1.2452
1.3244
1.2346
0.7037
13.9104
13.9044
13.8015
13.9704
2.6584
1.5203
1.5747
1.4742Table1 demonstrates that the system is completely observable and every state value can be estimated with the SVD theory. Particularly, εy, φz, φy, εx, and εz which have a larger observable degree can reach a better alignment effect. The observable degree of δVx, δVy, and δVz is almost 1. Compared with the other eleven states, the observable degree of φx 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:(39)Xk=Φk,k-1Xk-1+Γk-1Wk-1,(40)Zk=HkXk+Vk.The measurement residual is given by(41)δZk=Zk-HkX^k,k-1.By substituting (40) in (41), the measurement residual can be written as(42)δZk=HkXk-X^k,k-1+V1k+αV2k+χXk,X^k,k-1,where α is a real number, α≥0, V1k is the measurement noise with a fixed variance, and V2k is the time-varying measurement noise. It is assumed that the two types of measurement noises are uncorrelated. χ(Xk,X^k,k-1) is the higher order item in the estimation error. If this item is ignored, (42) can be simplified to(43)δZk=HkXk-X^k,k-1+V1k+αV2k.The covariance of the measurement residual is expressed as(44)Sk,k-1=EδZkδZkT=HkPk,k-1HkT+R1k+βR2k,where β=α2 is scalar value that accommodates the filter’s stability and optimality, R1k=E[V1kV1kT], and R2k=EV2kV2kT.The covariance of state estimation error is defined as(45)Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qd.Substituting (45) into (44) results in(46)Sk,k-1=HkΦk,k-1Pk-1Φk,k-1T+QdHkT+R1k+βR2k=L1+βL2,where L1=Hk(Φk,k-1Pk-1Φk,k-1T+Qd)HkT+R1k and L2=R2k.Thus we can get the optimal gain of the adaptive Kalman filter as follows:(47)Kk=Pk,k-1HkTSk,k-1-1.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:(48)Cjk=EδZk+jδZkT=HkPk,k-1HkT+R1k+βR2k=0j=1,2,…;k=0,1,….The autocovariance of the measurement residuals is(49)Cjk=EδZk+jδZkT=Hk+jϕk+1,k+j-1I-Kk+j-1Hk+j-1⋯ϕk+2,k+1I-Kk+1Hk+1·ϕk+1,kPk∣k-1HkT-KkC0k,∀j=1,2,3,….In reality, the real covariance of the measurement residuals is different from a theoretical one owing to model errors or unknown external disturbances. Thus,Cj(k) may not be identical to zero. From (49), we know that if the last term of Cj(k) which is the only common item of Cj(k) for all j=1,2,3,… is zero, (50)Pk∣k-1HkT-KkC0k=0,then the gain Kk is optimal. In other words, if the gain Kk is optimal, (50) holds. Equation (50) forms the basis for determining scalar value β. In particular, C0(k) in (50) which is computed from measured data in real time, according to k successive residuals, can be shown as(51)C0k=1k-1∑i=1kδZiδZiT.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.
Q
(
k
), R(k), and P(0) are all positive definite symmetrical matrix.Assumption 2.
The measurement matrixH(k) is full ranked:(52)β=tr⁡⁡C0k-L1tr⁡⁡L2,where if β≤0, β=0 and if β≥max⁡⁡β, β=max⁡⁡β.Proof.
Substituting (47) into the optimality condition (50) gives(53)Pk∣k-1HkTI-L1+βL2-1C0k=0.
SincePk∣k-1 and C0(k) are full-ranked symmetric matrix according to Assumptions 1 and 2 and (51), left multiplying Pk∣k-1-1 and right multiplying C0(k)-1 on both sides of (53), it is obvious that (53) implies the following relation:(54)HkTC0k-1-HkTL1+βL2-1=0;then(55)HkTC0k-1L1+βL2=HkT.
SinceHk is full-ranked matrix according to Assumption 2, (55) is simplified as(56)βL2=C0k-L1.
To avoid inversion manipulation, trace is directly taken in both sides of (56):(57)tr⁡⁡βL2=tr⁡⁡C0k-L1then 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:(58)X^k,k-1=Φk,k-1X^k-1,Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qd,L1=HkΦk,k-1Pk-1Φk,k-1T+QdHkT+R1k,L2=R2k,C0k=1k-1∑i=1kδZiδZiT,β=tr⁡⁡C0k-L1tr⁡⁡L2,Sk,k-1=HkPk,k-1HkT+R1k+βR2k,Kk=Pk,k-1HkTSk,k-1-1,Pk=I-KkHkPk,k-1,X^k=X^k,k-1+KkZk-HkX^k,k-1.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 U/D covariance factorization filter, where U is an upper triangular and unitary matrix and D is a diagonal matrix. It is also advantageous to utilize the U/D 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:(59)θ=7∘cos⁡⁡2π5t+π4γ=10∘cos⁡⁡2π6t+π7ψ=30∘+5∘cos⁡⁡2π7t+π3.The velocity caused by surge, sway, and heave is as follows:(60)VDi=ADiωDicos⁡⁡ωDit+φDi,where i=x,y,z, ADx=0.02m, ADy=0.03m, ADz=0.3m, ωDi=2π/TDi, TDx=7 s, TDy=6 s, and TDz=8 s. φDi obeys the uniform distribution on the interval [0,2π].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 oftk1 and tk2 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 Table2, 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 Table2 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.Table 2
Statistics for coarse alignment results.
Parameter item
Max.
Min.
Mean
Pitch error (°)
0.0740
-
0.0986
0.0287
Roll error (°)
0.1563
-
0.2668
-
0.0419
Heading error (°)
1.4611
-
0.1044
0.1506Simulation 2.
The simulation results are shown in Figure1. 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 ∇x and ∇y 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.Figure 1
The simulation results of NAKF.
(a)
Estimation of attitude errors
(b)
Estimation of velocity error
(c)
Estimation of gyro drifts
(d)
Estimation of accelerometer biasesSimulation 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 Simulation3 with the maximum initial figures is made.Table 3
The mean misalignment angle errors depending on the filter.
Parameter item
SHAF
AKF
NAKF
Pitch error (′)
-
1.43
-
1.17
1.02
Roll error (′)
-
1.62
-
1.29
1.07
Heading error (′)
1.85
1.54
-
1.26Figure 2
The results depending on the filter with mean misalignment angles.
(a)
Tilt misalignment angle
(b)
Eastern misalignment angle
(c)
Northern misalignment angleSimulation 4.
The simulation results show that the proposed NAKF is efficient with large initial attitude error. The results are shown in Figure3 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.Table 4
The maximum misalignment angle errors depending on the filter.
Parameter item
SHAF
AKF
NAKF
Pitch error (′)
2.93
2.59
-
1.89
Roll error (′)
3.09
-
2.29
1.80
Heading error (′)
3.81
-
2.14
1.84Figure 3
The results depending on the filter with maximum misalignment angles.
(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.



---

*Source: 320536-2015-10-04.xml*