# A Fast in-Motion Alignment Algorithm for DVL Aided SINS

Authors: Li Kang; Lingyun Ye; Kaichen Song
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

Doppler velocity log (DVL) aided strapdown inertial navigation system (SINS) is a common navigation method for underwater applications. Owing to the in-motion condition and the lack of the GPS, it is a challenge to align a SINS under water. This paper proposed a complete in-motion alignment solution for both attitude and position. The velocity update equation and its integral form in the body frame are studied, and the attitude coarse alignment becomes an optimization-based attitude determination problem between the body frame velocity and the integral form of gravity. The body frame velocity and the Earth frame position are separately treated, and the position alignment problem turns into an equation solving problem. Simulation and on-lake tests are carried out to examine the algorithm. The heading could reach around 10 deg accuracy and the pitch and roll could be aligned up to 0.05 deg in 60 s. With attitude error of this level, the heading could reach 1 deg accuracy in 240 s using unscented Kalman filter (UKF) based fine alignment. The final position error could achieve 1.5% of the voyage distance. This scheme can also be applied to other body frame velocity aided SINS alignments.


---

## Body

## 1. Introduction

DVL aided SINS is now widely used in the autonomous underwater vehicles (AUVs) [1–3]. It is a challenge to align a SINS under water for two reasons. The first is that the alignment needs to finish in-motion since the vehicle cannot keep stationary due to the water flow. The second is that the GPS [4, 5], the most useful aided sensor, which could provide geodetic frame velocity and the Earth frame position, is not available under water. So, it is necessary to develop DVL aided in-motion alignment. Due to the lack of position information, the alignment should include both attitude and position.The attitude alignment often includes two stages: coarse alignment and fine alignment. The attitude fine alignment is mainly based on the Earth frame position or geodetic velocity matching with Kalman filter (KF) [6]. Currently, the body frame velocity matching alignment could already partly solve the problem if a roughly known attitude could pass to the filter. But if the error of the roughly known attitude is too large, the alignment would take much time and the final results would be worse [7]. So the key process is to get this roughly known attitude. This process is called the attitude coarse alignment.Various algorithms for the coarse alignment have been developed. One kind of algorithms determined the attitude from gyroscope and accelerometer measurements using the gravity and the Earth’s rotation as a reference [6, 8, 9]. These algorithms are fit for the condition that the SINS is stationary or quasi-stationary. While in-motion condition the angular rate is much larger than the Earth’s rotation, the attitude error is large in most situations. Another kind of algorithms applied the velocity of the GPS as an input and optimization approach is used between the velocities of the navigation frame and the body frame [10, 11]. This method could offer a good result to the fine alignment in in-motion condition. But there is no GPS signal under water. Also it is not a good idea to get the initial attitude using a magnetic compass, since there are many electromagnetic interference sources such as the motors and the solenoid for the emergency jettison. The magnetic compass is not robust in such harsh environment.The task of the position alignment is to provide the position during the attitude alignment process. This is mainly provided by the GPS for on-land assignments [11], while the GPS is not available for underwater assignments. The main idea is to use the DVL to reckon the coordinate from the start point. The problem is that the body frame velocity cannot be integrated into position before the attitude is known.The contents are organized as follows. Section2 explains the problem in mathematics. Section 3 is the attitude alignment section. The attitude coarse alignment is devised by further study of the velocity update equation in the navigation frame and its transformation in the body frame. Optimization approach is applied between the body frame velocity and the integral form of the gravity. Also the common large misalignment angles model based UKF is reviewed in Section 3. Section 4 proposed the position alignment by separately treating the body frame velocity and the Earth frame position. And when the initial attitude is gathered, the position could be straight obtained through an equation. The performance of the algorithm is evaluated with simulation in Section 5 and on-lake tests in Section 6. Conclusions are drawn in Section 7.


## 2. Problem Statement

The coordinate frames used for the in-motion alignment are defined as follows.(i)
Then frame is the ideal local level navigation coordinate frame with east-north-up geodetic axes.(ii)
Then′ frame is the real local level navigation coordinate frame; there is some error between n and n′ frame owing to the alignment error and the sensor error.(iii)
Theb frame is the strapdown inertial sensor’s body coordinate frame.(iv)
Thee frame is the Earth coordinate frame.(v)
Thei frame is the nonrotating inertial coordinate frame.The common navigation (attitude, velocity, and position) update equations are written in then frame as [12–15]
(1)C˙bn=Cbnωnbb×,(2)v˙n=Cbnfb-(2ωien+ωenn)×vn+gn,(3)p˙=Rcvn,
where Cbn is the attitude direction cosine matrix (DCM) from the b frame to the n frame, vn is the velocity in the n frame, fb is the special force measured by the accelerometers fixed in the b frame, ωien is the angular rate of the Earth’s rotation in the n frame, ωenn is the angular rate caused by the velocity on the Earth (since the Earth is nearly round) in the n frame, gn is the gravity in the n frame, (·)× is the matrix form of a cross-product which satisfies a×b=(a×)b,p is the position described in the e frame as p=[λ,φ,h]T, λ is the longitude, φ is the latitude, h is the height above the mean sea level, R is the radius of the Earth, ωibb is the angular rate measured by the gyroscopes in the b frame, ωnbb is the angular rate of the SINS in the b frame with respect to the n frame described as
(4)ωnbb=ωibb-Cnb(ωien+ωenn),
and Rc is the velocity transformation matrix between the n frame and the e frame defined as
(5)Rc=[secφR+h0001R+h0001].The velocityvn can be measured by the DVL if Cbn is known. The purpose of the alignment is to determine the attitude DCM Cbn and the position p based on the data collected from the accelerometers, the gyroscopes, and the DVL while in motion.


## 3. Attitude Alignment

### 3.1. Attitude Coarse Alignment

Since the velocity of the DVL is in theb frame, the aim here is to convert (2) into the b frame too. Equation (2) can be written as
(6)v˙n=d(Cbnvb)dt=C˙bnvb+Cbnv˙b=Cbnfb-(2ωien+ωenn)×vn+gn.
Substituting (1) into (6) yields
(7)v˙b=fb-Cnb(2ωien+ωenn)×(Cbnvb)-ωnbb×vb+Cnbgn.
This is the complete form of the velocity update equation in the b frame. In fact, Cnb(2ωien+ωenn)×(Cbnvb) could be omitted for the reason that the AUVs would not sail in a very high speed (commonly several meters per second) and it is the coarse alignment stage. The simplified form of (7) for the coarse alignment is
(8)v˙b=fb-ωnbb×vb+Cnbgn.
Equation (1) is the traditional attitude update equation. Nowadays, the attitude update is separated using the DCM product chain rule as [12]
(9)Cb(t+Δt)n(t+Δt)=Cn(t)n(t+Δt)Cb(t)n(t)Cb(t+Δt)b(t).
The update method for Cn(t)n(t+Δt) and Cb(t+Δt)b(t) is also proposed in [12]. If this rule is applied in each update cycle, the attitude update equation will be
(10)Cb(t)n(t)=Cn(0)n(t)Cb(0)n(0)Cb(t)b(0).
The update equations for Cn(t)n(0) and Cb(t)b(0) are [11]
(11)C˙n(t)n(0)=Cn(t)n(0)ωinn×,(12)C˙b(t)b(0)=Cb(t)b(0)ωibb×,
where ωinn=ωien+ωenn≈ωien for the reason that ωenn is unknown but minor in alignment stage. If Cb(0)n(0) is identified, Cb(t)n(t) on every time step is known. The procedure identifying Cb(0)n(0) is as follows.Substitute (10) into (8) and multiply Cn(t)n(0) on each part
(13)Cb(t)b(0)v˙b-Cb(t)b(0)fb+Cb(t)b(0)ωnbb×vb=Cn(0)b(0)Cn(t)n(0)gn.
Integrating (13) on both sides
(14)α(t)=Cn(0)b(0)β(t),
where
(15)α(t)=∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vb-Cb(t)b(0)fbdt,(16)β(t)=∫0tCn(t)n(0)gndt.
The initial attitude matrix Cb(0)n(0) can be uniquely solved by the optimization-based method described in [16, 17] to minimize
(17)L(Cn(0)b(0))=∑(α(tN)-Cn(0)b(0)β(tN))2.
So the key is how to calculate α(t) and β(t) with different integral time.


### 3.2. Calculation Scheme forα(t), β(t)

Calculation scheme forα(t), β(t) includes two parts: integration strategy and update scheme.


#### 3.2.1. Integration Strategy

Ifv˙b, ωibb, and fb were known, (13) could be the model for calculating Cb(0)n(0). This cannot be achieved by real sensors. The DVL’s output is the velocity in the b frame, the gyroscope’s output is the incremental angle over the sample time, and the accelerometer’s output is the incremental velocity over the sample time. This is the reason that the integral forms are needed.For (15), notice that
(18)∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vbdt=Cb(t)b(0)vb|0t-∫0tC˙b(t)b(0)vb-Cb(t)b(0)ωnbb×vbdt.
Substitute (4) and (12) into (18) and omit the Cb(t)b(0)Cn(t)b(t)(ωien+ωenn)×vb part for the same reason as the AUVs would not voyage in a very high speed
(19)∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vbdt≈Cb(t)b(0)vb|0t.The calculation methods for the other part of (15) and the whole equation (16) are already expressed in [11] as
(20)∫0tCb(t)b(0)fbdt=∑k=0N-1Cb(tk)b(0)∫tktk+1Cb(t)b(tk)fbdt≈∑k=0N-1Cb(tk)b(0)∫tktk+1[I+(∫tktωibb×dτ)]fbdt=∑k=0N-1Cb(tk)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)],(21)β(tN)=∫0tCn(t)n(0)gndt=∑k=0N-1Cn(tk)n(0)∫tktk+1Cn(t)n(tk)gndt≈∑k=0N-1Cn(tk)n(0)∫tktk+1(I+(t-tk)ωinn×)gndt=∑k=0N-1Cn(tk)n(0)(TI+T22ωinn×)gn≈∑k=0N-1Cn(tk)n(0)(TI+T22ωien×)gn.
So
(22)α(tN)≈Cb(tN)b(0)vb(tN)-vb(0)-∑k=0N-1Cb(tk)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)],
where the angular rate and the specific force are assumed in linear forms within a short time as
(23)ωibb=(t-tk)cω+dω,fb=(t-tk)cf+df.
The cf, df, cω, and dω are the coefficient vectors as
(24)cf=4(Δv2-Δv1)T2,df=3Δv1-Δv2T,(25)cω=4(Δθ2-Δθ1)T2,dω=3Δθ1-Δθ2T,
where T is the time period of one update interval [tktk+1], k=0,1,2,…,N-1, with tk=kT, and Δv1, Δv2 and Δθ1, Δθ2 are the incremental velocity and angle collected from the accelerometers and gyroscopes.


#### 3.2.2. Update Scheme

The update rate of the sensors is not the same. The update rate of the gyroscopes and accelerometers can go up from 1 to 2 kHz, while the update rate of the DVL is only several Hz and the update may be interrupted due to the depth of the water or some other reasons. The update scheme forα(t), β(t) should be suitable for these conditions.Forα(t) expressed in (22), the lower part is related to the gyroscopes and accelerometers. In order to obtain an accurate result, this part should be updated at these sensors’ update rate. A higher update rate could reduce the effect such as coning motion [18]. For the whole equation, the update rate is based on the DVL. It means that the lower part should be updated in high speed and when the DVL’s data is available the whole equation is updated.Forβ(t) expressed in (21), the update rate is the same as the gyroscopes and accelerometers. And the output rate is kept the same as α(t) since the algorithm needs the data at the same time point. Consider
(26)α(t)⟶αDVL(tM),β(t)⟶βDVL(tM),
where M=0,1,2,…,tM means the time point that the DVL is available. The least number of M is decided by the sensor error level.Owing to the low and unsteady output rate of the DVL, thevb(0) is almost not obtainable. This part can be counteracted as
(27)αDVL(tM)-αDVL(t0)=Cn(0)b(0)(βDVL(tM)-βDVL(t0)).
This is the practical equation for the coarse alignment.


### 3.3. Attitude Fine Alignment

According to the sensor error level, about 200 samples(M≈200) of DVL data are needed for the coarse alignment. Since the update rate of the DVL is several Hz, the coarse alignment could cost about 60 s. After 60 s of the attitude coarse alignment, the pitch and roll would attain about 0.1 deg accuracy and the heading error would be around 10 deg. The fine alignment starts immediately after it. The goal is to improve the attitude accuracy especially the heading accuracy.For the fine alignment, most methods use the SINS attitude and velocity error models as the state transition equation for the data fusion algorithms such as the Kalman filter (KF), the extended KF (EKF), and the UKF. The UKF for its worthy performance in nonlinear estimation [19, 20] is especially used for nonlinear misalignment angles. Here is the review of the UKF based attitude fine alignment with nonlinear misalignment angles model [7, 21, 22].The navigation algorithm is described in (1), (2), and (3). The state of the UKF is
(28)x=[δvnθδωibbδfb]T.
And the large misalignment angles model is
(29)δv˙n=(I-Cn′n)Cbn′f^b-(2δωien+δωenn)×v^n-(2ω^ien+ω^enn)×δvn+Cn′nCbn′δfb,θ˙=(I-Cnn′)ω^inn+δωinn-Cbn′δωibb,δω˙ibb=0+εω,δf˙b=0+εf,
where y^ is the real data equal to the ideal data y plus the error δy, Cbn′ is the DCM from the b frame to the n′ frame, εω and εf are the noise of the gyroscopes and accelerometers, and Cnn′ is the DCM from the n frame to the n′ frame. Assume that the n frame to the n′ frame is completed through three noninterchangeable rotations θz, θx, and θy. Each rotation can be expressed as
(30)Cθx=[1000cos⁡θxsinθx0-sinθxcos⁡θx],Cθy=[cos⁡θy0-sinθy010sinθy0cos⁡θy],Cθz=[cos⁡θzsinθz0-sinθzcos⁡θz0001].
The total DCM is
(31)Cnn′=CθyCθxCθz.The measurement model is(32)z=v^n-Cn′nCbn′v^DVLb=vn+δvn-Cn′nCbn′(vDVLb+δvDVLb)=vn-vDVLn+δvn-δvDVLn=δvn-δvDVLn,
where δvDVLn is the noise of the DVL.Then according to (29) and (32), the nonlinear state and measurement equations are(33)x˙=[δv˙nθ˙δω˙ibbδf˙b]=f(x)+w=[(I-Cn′n)Cbn′f^b-(2δωien+δωenn)×v^n-(2ω^ien+ω^enn)×δvn+Cn′nCbn′δfb(I-Cnn′)ω^inn+δωinn-Cbn′δωibb00]+[00εωεf],z=h(x)+u=[δvn]+[-δvDVLn],where w=[03×103×1εωεf] is the state noise and u=[-δvDVLn] is the measurement noise.The fine alignment’s duty is to estimateCn′n to improve the accuracy of Cbn. Notice that Cb(0)n(0), Cb(t)b(0), and Cn(t)n(0) can still be updated in fine alignment. It means that a more precise Cb(0)n(0) is realized too according to the inverse of (10) in the fine alignment stage as
(34)Cb(0)n(0)=Cn(t)n(0)Cb(t)n(t)Cb(0)b(t).
This result is of great importance in the position alignment.


## 3.1. Attitude Coarse Alignment

Since the velocity of the DVL is in theb frame, the aim here is to convert (2) into the b frame too. Equation (2) can be written as
(6)v˙n=d(Cbnvb)dt=C˙bnvb+Cbnv˙b=Cbnfb-(2ωien+ωenn)×vn+gn.
Substituting (1) into (6) yields
(7)v˙b=fb-Cnb(2ωien+ωenn)×(Cbnvb)-ωnbb×vb+Cnbgn.
This is the complete form of the velocity update equation in the b frame. In fact, Cnb(2ωien+ωenn)×(Cbnvb) could be omitted for the reason that the AUVs would not sail in a very high speed (commonly several meters per second) and it is the coarse alignment stage. The simplified form of (7) for the coarse alignment is
(8)v˙b=fb-ωnbb×vb+Cnbgn.
Equation (1) is the traditional attitude update equation. Nowadays, the attitude update is separated using the DCM product chain rule as [12]
(9)Cb(t+Δt)n(t+Δt)=Cn(t)n(t+Δt)Cb(t)n(t)Cb(t+Δt)b(t).
The update method for Cn(t)n(t+Δt) and Cb(t+Δt)b(t) is also proposed in [12]. If this rule is applied in each update cycle, the attitude update equation will be
(10)Cb(t)n(t)=Cn(0)n(t)Cb(0)n(0)Cb(t)b(0).
The update equations for Cn(t)n(0) and Cb(t)b(0) are [11]
(11)C˙n(t)n(0)=Cn(t)n(0)ωinn×,(12)C˙b(t)b(0)=Cb(t)b(0)ωibb×,
where ωinn=ωien+ωenn≈ωien for the reason that ωenn is unknown but minor in alignment stage. If Cb(0)n(0) is identified, Cb(t)n(t) on every time step is known. The procedure identifying Cb(0)n(0) is as follows.Substitute (10) into (8) and multiply Cn(t)n(0) on each part
(13)Cb(t)b(0)v˙b-Cb(t)b(0)fb+Cb(t)b(0)ωnbb×vb=Cn(0)b(0)Cn(t)n(0)gn.
Integrating (13) on both sides
(14)α(t)=Cn(0)b(0)β(t),
where
(15)α(t)=∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vb-Cb(t)b(0)fbdt,(16)β(t)=∫0tCn(t)n(0)gndt.
The initial attitude matrix Cb(0)n(0) can be uniquely solved by the optimization-based method described in [16, 17] to minimize
(17)L(Cn(0)b(0))=∑(α(tN)-Cn(0)b(0)β(tN))2.
So the key is how to calculate α(t) and β(t) with different integral time.


## 3.2. Calculation Scheme forα(t), β(t)

Calculation scheme forα(t), β(t) includes two parts: integration strategy and update scheme.


### 3.2.1. Integration Strategy

Ifv˙b, ωibb, and fb were known, (13) could be the model for calculating Cb(0)n(0). This cannot be achieved by real sensors. The DVL’s output is the velocity in the b frame, the gyroscope’s output is the incremental angle over the sample time, and the accelerometer’s output is the incremental velocity over the sample time. This is the reason that the integral forms are needed.For (15), notice that
(18)∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vbdt=Cb(t)b(0)vb|0t-∫0tC˙b(t)b(0)vb-Cb(t)b(0)ωnbb×vbdt.
Substitute (4) and (12) into (18) and omit the Cb(t)b(0)Cn(t)b(t)(ωien+ωenn)×vb part for the same reason as the AUVs would not voyage in a very high speed
(19)∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vbdt≈Cb(t)b(0)vb|0t.The calculation methods for the other part of (15) and the whole equation (16) are already expressed in [11] as
(20)∫0tCb(t)b(0)fbdt=∑k=0N-1Cb(tk)b(0)∫tktk+1Cb(t)b(tk)fbdt≈∑k=0N-1Cb(tk)b(0)∫tktk+1[I+(∫tktωibb×dτ)]fbdt=∑k=0N-1Cb(tk)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)],(21)β(tN)=∫0tCn(t)n(0)gndt=∑k=0N-1Cn(tk)n(0)∫tktk+1Cn(t)n(tk)gndt≈∑k=0N-1Cn(tk)n(0)∫tktk+1(I+(t-tk)ωinn×)gndt=∑k=0N-1Cn(tk)n(0)(TI+T22ωinn×)gn≈∑k=0N-1Cn(tk)n(0)(TI+T22ωien×)gn.
So
(22)α(tN)≈Cb(tN)b(0)vb(tN)-vb(0)-∑k=0N-1Cb(tk)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)],
where the angular rate and the specific force are assumed in linear forms within a short time as
(23)ωibb=(t-tk)cω+dω,fb=(t-tk)cf+df.
The cf, df, cω, and dω are the coefficient vectors as
(24)cf=4(Δv2-Δv1)T2,df=3Δv1-Δv2T,(25)cω=4(Δθ2-Δθ1)T2,dω=3Δθ1-Δθ2T,
where T is the time period of one update interval [tktk+1], k=0,1,2,…,N-1, with tk=kT, and Δv1, Δv2 and Δθ1, Δθ2 are the incremental velocity and angle collected from the accelerometers and gyroscopes.


### 3.2.2. Update Scheme

The update rate of the sensors is not the same. The update rate of the gyroscopes and accelerometers can go up from 1 to 2 kHz, while the update rate of the DVL is only several Hz and the update may be interrupted due to the depth of the water or some other reasons. The update scheme forα(t), β(t) should be suitable for these conditions.Forα(t) expressed in (22), the lower part is related to the gyroscopes and accelerometers. In order to obtain an accurate result, this part should be updated at these sensors’ update rate. A higher update rate could reduce the effect such as coning motion [18]. For the whole equation, the update rate is based on the DVL. It means that the lower part should be updated in high speed and when the DVL’s data is available the whole equation is updated.Forβ(t) expressed in (21), the update rate is the same as the gyroscopes and accelerometers. And the output rate is kept the same as α(t) since the algorithm needs the data at the same time point. Consider
(26)α(t)⟶αDVL(tM),β(t)⟶βDVL(tM),
where M=0,1,2,…,tM means the time point that the DVL is available. The least number of M is decided by the sensor error level.Owing to the low and unsteady output rate of the DVL, thevb(0) is almost not obtainable. This part can be counteracted as
(27)αDVL(tM)-αDVL(t0)=Cn(0)b(0)(βDVL(tM)-βDVL(t0)).
This is the practical equation for the coarse alignment.


## 3.2.1. Integration Strategy

Ifv˙b, ωibb, and fb were known, (13) could be the model for calculating Cb(0)n(0). This cannot be achieved by real sensors. The DVL’s output is the velocity in the b frame, the gyroscope’s output is the incremental angle over the sample time, and the accelerometer’s output is the incremental velocity over the sample time. This is the reason that the integral forms are needed.For (15), notice that
(18)∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vbdt=Cb(t)b(0)vb|0t-∫0tC˙b(t)b(0)vb-Cb(t)b(0)ωnbb×vbdt.
Substitute (4) and (12) into (18) and omit the Cb(t)b(0)Cn(t)b(t)(ωien+ωenn)×vb part for the same reason as the AUVs would not voyage in a very high speed
(19)∫0tCb(t)b(0)v˙b+Cb(t)b(0)ωnbb×vbdt≈Cb(t)b(0)vb|0t.The calculation methods for the other part of (15) and the whole equation (16) are already expressed in [11] as
(20)∫0tCb(t)b(0)fbdt=∑k=0N-1Cb(tk)b(0)∫tktk+1Cb(t)b(tk)fbdt≈∑k=0N-1Cb(tk)b(0)∫tktk+1[I+(∫tktωibb×dτ)]fbdt=∑k=0N-1Cb(tk)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)],(21)β(tN)=∫0tCn(t)n(0)gndt=∑k=0N-1Cn(tk)n(0)∫tktk+1Cn(t)n(tk)gndt≈∑k=0N-1Cn(tk)n(0)∫tktk+1(I+(t-tk)ωinn×)gndt=∑k=0N-1Cn(tk)n(0)(TI+T22ωinn×)gn≈∑k=0N-1Cn(tk)n(0)(TI+T22ωien×)gn.
So
(22)α(tN)≈Cb(tN)b(0)vb(tN)-vb(0)-∑k=0N-1Cb(tk)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)],
where the angular rate and the specific force are assumed in linear forms within a short time as
(23)ωibb=(t-tk)cω+dω,fb=(t-tk)cf+df.
The cf, df, cω, and dω are the coefficient vectors as
(24)cf=4(Δv2-Δv1)T2,df=3Δv1-Δv2T,(25)cω=4(Δθ2-Δθ1)T2,dω=3Δθ1-Δθ2T,
where T is the time period of one update interval [tktk+1], k=0,1,2,…,N-1, with tk=kT, and Δv1, Δv2 and Δθ1, Δθ2 are the incremental velocity and angle collected from the accelerometers and gyroscopes.


## 3.2.2. Update Scheme

The update rate of the sensors is not the same. The update rate of the gyroscopes and accelerometers can go up from 1 to 2 kHz, while the update rate of the DVL is only several Hz and the update may be interrupted due to the depth of the water or some other reasons. The update scheme forα(t), β(t) should be suitable for these conditions.Forα(t) expressed in (22), the lower part is related to the gyroscopes and accelerometers. In order to obtain an accurate result, this part should be updated at these sensors’ update rate. A higher update rate could reduce the effect such as coning motion [18]. For the whole equation, the update rate is based on the DVL. It means that the lower part should be updated in high speed and when the DVL’s data is available the whole equation is updated.Forβ(t) expressed in (21), the update rate is the same as the gyroscopes and accelerometers. And the output rate is kept the same as α(t) since the algorithm needs the data at the same time point. Consider
(26)α(t)⟶αDVL(tM),β(t)⟶βDVL(tM),
where M=0,1,2,…,tM means the time point that the DVL is available. The least number of M is decided by the sensor error level.Owing to the low and unsteady output rate of the DVL, thevb(0) is almost not obtainable. This part can be counteracted as
(27)αDVL(tM)-αDVL(t0)=Cn(0)b(0)(βDVL(tM)-βDVL(t0)).
This is the practical equation for the coarse alignment.


## 3.3. Attitude Fine Alignment

According to the sensor error level, about 200 samples(M≈200) of DVL data are needed for the coarse alignment. Since the update rate of the DVL is several Hz, the coarse alignment could cost about 60 s. After 60 s of the attitude coarse alignment, the pitch and roll would attain about 0.1 deg accuracy and the heading error would be around 10 deg. The fine alignment starts immediately after it. The goal is to improve the attitude accuracy especially the heading accuracy.For the fine alignment, most methods use the SINS attitude and velocity error models as the state transition equation for the data fusion algorithms such as the Kalman filter (KF), the extended KF (EKF), and the UKF. The UKF for its worthy performance in nonlinear estimation [19, 20] is especially used for nonlinear misalignment angles. Here is the review of the UKF based attitude fine alignment with nonlinear misalignment angles model [7, 21, 22].The navigation algorithm is described in (1), (2), and (3). The state of the UKF is
(28)x=[δvnθδωibbδfb]T.
And the large misalignment angles model is
(29)δv˙n=(I-Cn′n)Cbn′f^b-(2δωien+δωenn)×v^n-(2ω^ien+ω^enn)×δvn+Cn′nCbn′δfb,θ˙=(I-Cnn′)ω^inn+δωinn-Cbn′δωibb,δω˙ibb=0+εω,δf˙b=0+εf,
where y^ is the real data equal to the ideal data y plus the error δy, Cbn′ is the DCM from the b frame to the n′ frame, εω and εf are the noise of the gyroscopes and accelerometers, and Cnn′ is the DCM from the n frame to the n′ frame. Assume that the n frame to the n′ frame is completed through three noninterchangeable rotations θz, θx, and θy. Each rotation can be expressed as
(30)Cθx=[1000cos⁡θxsinθx0-sinθxcos⁡θx],Cθy=[cos⁡θy0-sinθy010sinθy0cos⁡θy],Cθz=[cos⁡θzsinθz0-sinθzcos⁡θz0001].
The total DCM is
(31)Cnn′=CθyCθxCθz.The measurement model is(32)z=v^n-Cn′nCbn′v^DVLb=vn+δvn-Cn′nCbn′(vDVLb+δvDVLb)=vn-vDVLn+δvn-δvDVLn=δvn-δvDVLn,
where δvDVLn is the noise of the DVL.Then according to (29) and (32), the nonlinear state and measurement equations are(33)x˙=[δv˙nθ˙δω˙ibbδf˙b]=f(x)+w=[(I-Cn′n)Cbn′f^b-(2δωien+δωenn)×v^n-(2ω^ien+ω^enn)×δvn+Cn′nCbn′δfb(I-Cnn′)ω^inn+δωinn-Cbn′δωibb00]+[00εωεf],z=h(x)+u=[δvn]+[-δvDVLn],where w=[03×103×1εωεf] is the state noise and u=[-δvDVLn] is the measurement noise.The fine alignment’s duty is to estimateCn′n to improve the accuracy of Cbn. Notice that Cb(0)n(0), Cb(t)b(0), and Cn(t)n(0) can still be updated in fine alignment. It means that a more precise Cb(0)n(0) is realized too according to the inverse of (10) in the fine alignment stage as
(34)Cb(0)n(0)=Cn(t)n(0)Cb(t)n(t)Cb(0)b(t).
This result is of great importance in the position alignment.


## 4. Position Alignment

### 4.1. Position Alignment Algorithm

The original position update equation is described in (3). This position update equation can also be performed between the e frame and the b frame as
(35)Cn(t)n(0)Rc-1p˙(t)=Cb(0)n(0)Cb(t)b(0)vb(t).
In order to get the position, (35) should be integrated on each side first. The integral form of the left term is
(36)∫0tendCn(t)n(0)Rc-1p˙(t)dt=Cn(t)n(0)Rc-1p(t)|0tend-∫0tendC˙n(t)n(0)Rc-1p(t)dt,
where tend is the time point where the attitude alignment ends. Substitute (11) into (36):
(37)∫0tendCn(t)n(0)Rc-1p˙(t)dt=Cn(t)n(0)Rc-1p(t)|0tend-∫0tendCn(t)n(0)ωinn×Rc-1p(t)dt.
The integral part of (37) can be converted as
(38)∫0tendCn(t)n(0)ωinn×Rc-1p(t)dt=∑k=0N-1Cn(tk)n(0)∫tktk+1Cn(t)n(tk)ωinn×Rc-1p(t)dt,
where tk=tend, when k=N. Practically, the effort of the position change is really small in the integration. It could be regarded as constant as
(39)p(t)=p(tend),
where p(tend) is the position which needs alignment. Substitute (39) into the integral part of (38):
(40)∫tktk+1Cn(t)n(tk)ωinn×Rc-1p(t)dt≈∫tktk+1(I+(t-tk)ωinn×)ωinn×Rc-1p(tend)dt=ωinn×Rc-1p(tend)T+ωinn×ωinn×Rc-1p(tend)T22.
Therefore the computed form of (36) is
(41)∫0tendCn(t)n(0)Rc-1p˙dt≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-∑k=0N-1Cn(tk)n(0){+ωinn×ωinn×Rc-1p(tend)T22}ωinn×Rc-1p(tend)T+ωinn×ωinn×Rc-1p(tend)T22}.
For (41), ωinn×ωinn×Rc-1p(tend)T2/2, which is a second-order term, is much smaller than ωinn×Rc-1p(tend)T with T<1. So this part could be omitted. Then, notice that Tωinn×≈Cn(tk+1)n(tk)-I; (41) can be simplified as
(42)∫0tendCn(t)n(0)Rc-1p˙dt≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-∑k=0N-1Cn(tk)n(0){(Cn(tk+1)n(tk)-I)Rc-1p(tend)}≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-[Cn(tend)n(0)Rc-1p(tend)-Rc-1p(tend)]=Rc-1p(tend)-Rc-1p(0).
The integral form of the right term of (35) is
(43)∫0tendCb(0)n(0)Cb(t)b(0)vbdt=Cb(0)n(0)∑j=0M-1Cb(tj)b(0)∫tjtj+1Cb(t)b(tj)vbdt≈Cb(0)n(0)∑j=0M-1Cb(tj)b(0)∫tjtj+1(I+(∫tjtωibbdτ)×)vbdt,
where tj=tend, when j=M. Similarly, vb can be approximated linearly as
(44)vb(t)=vb(tj)+t-tjTDVL(vb(tj+1)-vb(tj)),
where TDVL is the duration of two DVL samples. Substitute (23), (25), and (44) into (43):
(45)∫0tendCb(0)n(0)Cb(t)b(0)vbdt≈Cb(0)n(0)∑j=0M-1Cb(tj)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1].
According to (35), (42) equals (45). It follows that
(46)p(tend)=p(0)+RcCb(0)n(0)∑j=0M-1Cb(tj)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))].For (46), all the data can be calculated in real-time except Cb(0)n(0). It means that when the initial attitude matrix Cb(0)n(0) is obtained, the position could immediately be updated. As described in Section 3, Cb(0)n(0) can be gathered by (27) through the attitude coarse alignment, and then the accuracy can be improved by (34) through the whole attitude fine alignment.Also, (39) can be treated as
(47)p(t)=p(0).
And then, (41) goes to
(48)∫0tendCn(t)n(0)Rc-1p˙dt≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-∑k=0N-1Cn(tk)n(0){×ωinn×Rc-1p(0)T22}ωinn×Rc-1p(0)T+ωinn×ωinn×Rc-1p(0)T22}.
And by making (48) be equal to (45), another solution comes out as
(49)Cn(tend)n(0)Rc-1p(tend)≈Cb(0)n(0)∑k=0N-1Cb(tk)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1]+Rc-1p(0)+∑k=0N-1Cn(tk)n(0)ωinn×Rc-1p(0)T+∑k=0N-1Cn(tk)n(0)ωinn×ωinn×Rc-1p(0)T22.
And we can also let (41) be equal to (45) to get another solution as
(50)Cn(tend)n(0)Rc-1p(tend)≈Cb(0)n(0)∑k=0N-1Cb(tk)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1]+Rc-1p(0)+∑k=0N-1Cn(tk)n(0)ωinn×Rc-1p(tend)T+∑k=0N-1Cn(tk)n(0)ωinn×ωinn×Rc-1p(tend)T22.
Although the expression of (49) and (50) is a little complicated, the alignment results are almost the same by the three different approximations. This will be approved in the simulation section.


### 4.2. In-Motion Alignment Summarization

A summarization of in-motion alignment for DVL aided SINS is proposed to give a better understanding of the whole algorithm.Step 1.
SetCn(0)n(0)=Cb(0)b(0)=I, p(0) as the beginning position coordinate.Step 2.
UpdateCn(t)n(0), Cb(t)b(0), (21), and the lower part of (22) when the data of the inertial sensors is available.Step 3.
Update the whole equation of (22) when the DVL’s data is available.Step 4.
Compute the initial attitude matrixCb(0)n(0) based on (27). Compute the attitude matrix Cb(t)n(t) by (10).Step 5.
Update the position via (46).Step 6.
Go to Step2 until the attitude coarse alignment is complete.Step 7.
Use the UKF based fine alignment algorithm. The navigation algorithm is described in (1), (2), and (3). The state and measurement equations are described in (33).Step 8.
UpdateCn(t)n(0), Cb(t)b(0) when the data of inertial sensors is available.Step 9.
Compute a more accurateCb(0)n(0) according to (34).Step 10.
Update the position via (46).Step 11.
Go to Step7 until the attitude fine alignment is complete.It is clear from these steps that the alignment can work on a real-time SINS, and no backtrack is needed.


## 4.1. Position Alignment Algorithm

The original position update equation is described in (3). This position update equation can also be performed between the e frame and the b frame as
(35)Cn(t)n(0)Rc-1p˙(t)=Cb(0)n(0)Cb(t)b(0)vb(t).
In order to get the position, (35) should be integrated on each side first. The integral form of the left term is
(36)∫0tendCn(t)n(0)Rc-1p˙(t)dt=Cn(t)n(0)Rc-1p(t)|0tend-∫0tendC˙n(t)n(0)Rc-1p(t)dt,
where tend is the time point where the attitude alignment ends. Substitute (11) into (36):
(37)∫0tendCn(t)n(0)Rc-1p˙(t)dt=Cn(t)n(0)Rc-1p(t)|0tend-∫0tendCn(t)n(0)ωinn×Rc-1p(t)dt.
The integral part of (37) can be converted as
(38)∫0tendCn(t)n(0)ωinn×Rc-1p(t)dt=∑k=0N-1Cn(tk)n(0)∫tktk+1Cn(t)n(tk)ωinn×Rc-1p(t)dt,
where tk=tend, when k=N. Practically, the effort of the position change is really small in the integration. It could be regarded as constant as
(39)p(t)=p(tend),
where p(tend) is the position which needs alignment. Substitute (39) into the integral part of (38):
(40)∫tktk+1Cn(t)n(tk)ωinn×Rc-1p(t)dt≈∫tktk+1(I+(t-tk)ωinn×)ωinn×Rc-1p(tend)dt=ωinn×Rc-1p(tend)T+ωinn×ωinn×Rc-1p(tend)T22.
Therefore the computed form of (36) is
(41)∫0tendCn(t)n(0)Rc-1p˙dt≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-∑k=0N-1Cn(tk)n(0){+ωinn×ωinn×Rc-1p(tend)T22}ωinn×Rc-1p(tend)T+ωinn×ωinn×Rc-1p(tend)T22}.
For (41), ωinn×ωinn×Rc-1p(tend)T2/2, which is a second-order term, is much smaller than ωinn×Rc-1p(tend)T with T<1. So this part could be omitted. Then, notice that Tωinn×≈Cn(tk+1)n(tk)-I; (41) can be simplified as
(42)∫0tendCn(t)n(0)Rc-1p˙dt≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-∑k=0N-1Cn(tk)n(0){(Cn(tk+1)n(tk)-I)Rc-1p(tend)}≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-[Cn(tend)n(0)Rc-1p(tend)-Rc-1p(tend)]=Rc-1p(tend)-Rc-1p(0).
The integral form of the right term of (35) is
(43)∫0tendCb(0)n(0)Cb(t)b(0)vbdt=Cb(0)n(0)∑j=0M-1Cb(tj)b(0)∫tjtj+1Cb(t)b(tj)vbdt≈Cb(0)n(0)∑j=0M-1Cb(tj)b(0)∫tjtj+1(I+(∫tjtωibbdτ)×)vbdt,
where tj=tend, when j=M. Similarly, vb can be approximated linearly as
(44)vb(t)=vb(tj)+t-tjTDVL(vb(tj+1)-vb(tj)),
where TDVL is the duration of two DVL samples. Substitute (23), (25), and (44) into (43):
(45)∫0tendCb(0)n(0)Cb(t)b(0)vbdt≈Cb(0)n(0)∑j=0M-1Cb(tj)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1].
According to (35), (42) equals (45). It follows that
(46)p(tend)=p(0)+RcCb(0)n(0)∑j=0M-1Cb(tj)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))].For (46), all the data can be calculated in real-time except Cb(0)n(0). It means that when the initial attitude matrix Cb(0)n(0) is obtained, the position could immediately be updated. As described in Section 3, Cb(0)n(0) can be gathered by (27) through the attitude coarse alignment, and then the accuracy can be improved by (34) through the whole attitude fine alignment.Also, (39) can be treated as
(47)p(t)=p(0).
And then, (41) goes to
(48)∫0tendCn(t)n(0)Rc-1p˙dt≈Cn(tend)n(0)Rc-1p(tend)-Rc-1p(0)-∑k=0N-1Cn(tk)n(0){×ωinn×Rc-1p(0)T22}ωinn×Rc-1p(0)T+ωinn×ωinn×Rc-1p(0)T22}.
And by making (48) be equal to (45), another solution comes out as
(49)Cn(tend)n(0)Rc-1p(tend)≈Cb(0)n(0)∑k=0N-1Cb(tk)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1]+Rc-1p(0)+∑k=0N-1Cn(tk)n(0)ωinn×Rc-1p(0)T+∑k=0N-1Cn(tk)n(0)ωinn×ωinn×Rc-1p(0)T22.
And we can also let (41) be equal to (45) to get another solution as
(50)Cn(tend)n(0)Rc-1p(tend)≈Cb(0)n(0)∑k=0N-1Cb(tk)b(0)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1×TDVLvb(tk)+(12Δθ1+16Δθ2)×TDVLvb(tk+1)[TDVL2(vb(tk+1)+vb(tk))+13Δθ1]+Rc-1p(0)+∑k=0N-1Cn(tk)n(0)ωinn×Rc-1p(tend)T+∑k=0N-1Cn(tk)n(0)ωinn×ωinn×Rc-1p(tend)T22.
Although the expression of (49) and (50) is a little complicated, the alignment results are almost the same by the three different approximations. This will be approved in the simulation section.


## 4.2. In-Motion Alignment Summarization

A summarization of in-motion alignment for DVL aided SINS is proposed to give a better understanding of the whole algorithm.Step 1.
SetCn(0)n(0)=Cb(0)b(0)=I, p(0) as the beginning position coordinate.Step 2.
UpdateCn(t)n(0), Cb(t)b(0), (21), and the lower part of (22) when the data of the inertial sensors is available.Step 3.
Update the whole equation of (22) when the DVL’s data is available.Step 4.
Compute the initial attitude matrixCb(0)n(0) based on (27). Compute the attitude matrix Cb(t)n(t) by (10).Step 5.
Update the position via (46).Step 6.
Go to Step2 until the attitude coarse alignment is complete.Step 7.
Use the UKF based fine alignment algorithm. The navigation algorithm is described in (1), (2), and (3). The state and measurement equations are described in (33).Step 8.
UpdateCn(t)n(0), Cb(t)b(0) when the data of inertial sensors is available.Step 9.
Compute a more accurateCb(0)n(0) according to (34).Step 10.
Update the position via (46).Step 11.
Go to Step7 until the attitude fine alignment is complete.It is clear from these steps that the alignment can work on a real-time SINS, and no backtrack is needed.


## 5. Simulation Results

The attitude coarse alignment and position alignment algorithms are examined in this section through simulation with four types of trajectories.The simulation trajectories’ base latitude and longitude are 30 deg and 120 deg. The pitch, roll, and heading are sinusoid waves with random amplitude and frequency within 10 deg and 1 Hz. Other parameters are listed below:trajectory 1 (Tr1): zero velocity condition;trajectory 2 (Tr2): random sinusoid wave condition with max velocity about 1 m/s;trajectory 3 (Tr3): line condition with random velocity around 1.4 m/s;trajectory 4 (Tr4): line condition with random velocity around 14 m/s.The first simulation uses error-free gyroscopes, accelerometers, and DVL data. Each type runs 50 times. Figure1 shows the average attitude errors. The pitch and roll errors reach 0.003 deg within 20 s for all four trajectories. And the heading errors reduce to 0.05 deg for Tr1 and Tr2 after 30 s alignment. And the heading errors grow to 0.1 deg for Tr3 and 1 deg for Tr4. This is mainly because that, with the velocity increasing, the omitted parts are growing too. And these parts are affecting the heading alignment accuracy.Average attitude errors of four types of trajectories.
(a)
(b)
(c)Figure2 gives the results where the omitted parts are included. It is clear that the heading accuracy increased. We hope that the alignment algorithm is perfect with all parts being considered. But the omitted parts are unknown since these parts need velocity in the n frame. And for the real sensors, the sensor errors would affect the accuracy much more than the omitted parts.Figure 2
Average heading errors of Tr4 and Tr4 with the omitted parts kept (Tr4_no).Figure3 illustrates the alignment errors with sensor errors (gyroscopes drift: 0.2 deg/h, gyroscopes noise: 0.9 deg/h/√Hz, accelerometers bias: 1×10-4 g, accelerometers noise: 2×10-4 g/√Hz, DVL bias: 1.5% of voyage velocity, and DVL noise: standard variance 0.02 m/s). The pitch and roll errors are almost the same compared with the error-free sensors, while the heading error’s 3σ envelope is 10 times larger than the error-free sensors. And it is also larger than the heading error of Tr4. So, the largest attitude coarse alignment error source is the sensor errors. Meanwhile, owing to the difference of the sensor error level, the convergence speed of Tr3_SE is slower than Tr3. But the heading error of Tr3_SE can still be less than 20 deg after 60 s. It is enough for the fine alignment stage.Average attitude errors of Tr3 and Tr3 with sensor errors (Tr3_SE). Dashed lines are the3σ envelopes.
(a)
(b)
(c)For the position alignment, three different position approximate methods are applied in Section4.Approximation 1 (Ap1): regardp(t) as p(tend), and ωinn×ωinn×Rc-1p(tend)T2/2 part is omitted.Approximation 2 (Ap 2): regardp(t) as p(0).Approximation 3 (Ap 3): regardp(t) as p(tend).Figure4 displays the average position errors of these three different approximations. The simulation still runs 50 times with four types of trajectories.Average position errors of four types of trajectories with three different position approximate methods.
(a)
(b)
(c)
(d)As shown in Figure4, the position errors of three different approximations are almost the same. It means that the approximation for (38) is reasonable. The position errors have a significant increase and decrease process in the first 20 s; this is mainly because the accuracy of the initial attitude DCM Cb(0)n(0) is increasing as described in Section 3.3. The max position errors of Tr4 are larger than the other trajectories for the reason that the velocity is at least 10 times larger and the attitude error is 10 times larger too. And the accuracy of Ap1 is especially a little bit higher than Ap2 and Ap3. So, Ap1 is applied in the next simulation and experiments.Figure5 demonstrates the average position errors with sensor errors. The position errors significantly increase. Still the largest error source is the sensor errors.Figure 5
Average position errors of Tr3 with Ap1 and Ap1 with sensor errors (Ap1_SE). Dashed lines are the3σ envelopes.


## 6. Experiment Results

### 6.1. Experiment Setup

The in-motion alignment experiment data was collected in the Thousand Island Lake in Zhejiang Province in China by using a towed body shown in Figure6. The sensors used for the experiment are listed below.(1)
An inertial navigation system developed by Zhejiang University with four fiber optics gyroscopes (bias instability: 0.2 deg/h) and four quartz accelerometers (bias:1×10-4 g). The data output rate is 500 Hz.(2)
A 600 kHz Phased Array ExplorerDVL produced by RD Instruments. Owing to the export restrictions to China, the accuracy is limited to about 1.5% of voyage. The data output rate is 4~5 Hz.(3)
An OEMV-1G GPS receiver produced by Novatel. The position accuracy is about 3 m. The velocity accuracy is about 0.1 m/s. The data output rate is 5 Hz.Figure 6
The towed body with an INS, a DVL, and a GPS.The GPS is fixed on the top of the towed body, while the INS and DVL are installed under water for about 0.3 m. The time delay and the lever arm of the sensors are compensated.


### 6.2. In-Motion Alignment Results

The trajectory of the towed body is shown in Figure7. Four alignments were done in the whole voyage. Each test includes 60 s coarse alignment, 240 s fine alignment, and 300 s navigation. The GPS aided INS was already aligned before the experiment and was working at navigation mode to produce standard position, velocity, and attitude. It was set as a standard system for the results comparison.Figure 7
The route of the towed body.The attitude alignment results are presented in Figures8, 9, and 10. The heading error reaches 20 deg within maximum 52 s (alignment 2) and minimum 24 s (alignment 3) and oscillates in this range during the coarse alignment. The heading error at 60 s is −5.2 deg, 13.7 deg, 4.8 deg, and 5.8 deg. This is consistent with the simulation results shown in Figure 3. The convergent speed of the coarse alignment is fast, but owing to the lack of the optimization approach method which cannot handle the noises of the sensors separately, the error just oscillates. This is why the fine alignment is required. With heading error of this level, it costs minimum 152 s (alignment 1) and maximum 225 s (alignment 3) to reach 1 deg of accuracy in the fine alignment stage. The errors of pitch and roll are less than 0.05 deg after the coarse alignment and keep in this range. So we believe that the total alignment could finish within 300 s.The heading error of the alignment. (a) Overview. (b) Details of the coarse alignment. (c) Details of the fine alignment.
(a)
(b)
(c)Figure 9
The pitch error of the alignment.Figure 10
The roll error of the alignments.The position alignment results are presented in Figures11 and 12. The towed body voyaged at the speed of about 1 m/s. The total voyage distance for each test was about 600 m. The error of the four alignments is 3 m, 16 m, 7 m, and 5 m after the coarse alignment. This is consistent with the simulation results shown in Figure 5. The error reduced to 1 m, 4 m, 2 m, and 3 m after the fine alignment. It is less than 1.5% of the voyage distance. The position error grows slowly after the alignment. This is another evidence to prove that the alignment is effective.Figure 11
Position error of the alignments.Figure 12
Position error/voyage distance of the alignments.Figures13 and 14 choose alignment 4 to show a comparison between the algorithm proposed in this paper (IMA1) and the previous UKF based method reviewed in Section 3.3 (IMA2). As described in [7], the UKF based method could solve the attitude alignment problem if the initial heading error is within about 100 deg. But if without the coarse alignment process, the heading error is unknown. So, the initial heading errors larger (150 deg) and smaller (20 deg) than 100 deg are all considered for IMA2 in the comparison.Figure 13
The comparison of the heading error of IMA1 and IMA2.Figure 14
The comparison of the position error of IMA1 and IMA2.If the initial heading error is small (IMA2-20), the heading error after the alignment is at the same level as IMA1, while if the initial heading error is large (IMA2-150), the heading may appear to be larger than 1 deg after the alignment without the attitude coarse alignment process. Also the position error is larger than 6% of the voyage distance which is 3 times larger than IMA1.


## 6.1. Experiment Setup

The in-motion alignment experiment data was collected in the Thousand Island Lake in Zhejiang Province in China by using a towed body shown in Figure6. The sensors used for the experiment are listed below.(1)
An inertial navigation system developed by Zhejiang University with four fiber optics gyroscopes (bias instability: 0.2 deg/h) and four quartz accelerometers (bias:1×10-4 g). The data output rate is 500 Hz.(2)
A 600 kHz Phased Array ExplorerDVL produced by RD Instruments. Owing to the export restrictions to China, the accuracy is limited to about 1.5% of voyage. The data output rate is 4~5 Hz.(3)
An OEMV-1G GPS receiver produced by Novatel. The position accuracy is about 3 m. The velocity accuracy is about 0.1 m/s. The data output rate is 5 Hz.Figure 6
The towed body with an INS, a DVL, and a GPS.The GPS is fixed on the top of the towed body, while the INS and DVL are installed under water for about 0.3 m. The time delay and the lever arm of the sensors are compensated.


## 6.2. In-Motion Alignment Results

The trajectory of the towed body is shown in Figure7. Four alignments were done in the whole voyage. Each test includes 60 s coarse alignment, 240 s fine alignment, and 300 s navigation. The GPS aided INS was already aligned before the experiment and was working at navigation mode to produce standard position, velocity, and attitude. It was set as a standard system for the results comparison.Figure 7
The route of the towed body.The attitude alignment results are presented in Figures8, 9, and 10. The heading error reaches 20 deg within maximum 52 s (alignment 2) and minimum 24 s (alignment 3) and oscillates in this range during the coarse alignment. The heading error at 60 s is −5.2 deg, 13.7 deg, 4.8 deg, and 5.8 deg. This is consistent with the simulation results shown in Figure 3. The convergent speed of the coarse alignment is fast, but owing to the lack of the optimization approach method which cannot handle the noises of the sensors separately, the error just oscillates. This is why the fine alignment is required. With heading error of this level, it costs minimum 152 s (alignment 1) and maximum 225 s (alignment 3) to reach 1 deg of accuracy in the fine alignment stage. The errors of pitch and roll are less than 0.05 deg after the coarse alignment and keep in this range. So we believe that the total alignment could finish within 300 s.The heading error of the alignment. (a) Overview. (b) Details of the coarse alignment. (c) Details of the fine alignment.
(a)
(b)
(c)Figure 9
The pitch error of the alignment.Figure 10
The roll error of the alignments.The position alignment results are presented in Figures11 and 12. The towed body voyaged at the speed of about 1 m/s. The total voyage distance for each test was about 600 m. The error of the four alignments is 3 m, 16 m, 7 m, and 5 m after the coarse alignment. This is consistent with the simulation results shown in Figure 5. The error reduced to 1 m, 4 m, 2 m, and 3 m after the fine alignment. It is less than 1.5% of the voyage distance. The position error grows slowly after the alignment. This is another evidence to prove that the alignment is effective.Figure 11
Position error of the alignments.Figure 12
Position error/voyage distance of the alignments.Figures13 and 14 choose alignment 4 to show a comparison between the algorithm proposed in this paper (IMA1) and the previous UKF based method reviewed in Section 3.3 (IMA2). As described in [7], the UKF based method could solve the attitude alignment problem if the initial heading error is within about 100 deg. But if without the coarse alignment process, the heading error is unknown. So, the initial heading errors larger (150 deg) and smaller (20 deg) than 100 deg are all considered for IMA2 in the comparison.Figure 13
The comparison of the heading error of IMA1 and IMA2.Figure 14
The comparison of the position error of IMA1 and IMA2.If the initial heading error is small (IMA2-20), the heading error after the alignment is at the same level as IMA1, while if the initial heading error is large (IMA2-150), the heading may appear to be larger than 1 deg after the alignment without the attitude coarse alignment process. Also the position error is larger than 6% of the voyage distance which is 3 times larger than IMA1.


## 7. Conclusions

The accuracy and applicability of SINS largely depended on the precision and swiftness of the alignment. It is a difficult problem to align an in-motion SINS with underwater applications due to the lack of the GPS which could provide velocity in then frame and position in the e frame. The alignment problem for underwater applications is not only the attitude alignment but also the position alignment.This paper proposed a complete alignment solution for DVL aided SINS in-motion by using the integration form of the velocity update equation in body frame to give a rough initial angle for the UKF based fine alignment and using the integration form of position update equation to alignment of the position. Simulation and on-lake tests were carried out to verify the alignment. The results show that the heading could reach around 10 deg accuracy and the pitch and roll could be aligned up to 0.05 deg during the coarse alignment. The heading could reach 1 deg accuracy in 240 s using UKF based fine alignment with heading error of this level. And the position error could achieve 1.5% of the voyage distance.



---

*Source: 593692-2014-06-04.xml*