A Fast in-Motion Alignment Algorithm for DVL Aided SINS

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.


Introduction
DVL aided SINS is now widely used in the autonomous underwater vehicles (AUVs) [1][2][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.Section 2 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.

Problem Statement
The coordinate frames used for the in-motion alignment are defined as follows.
(i) The  frame is the ideal local level navigation coordinate frame with east-north-up geodetic axes.
(ii) The   frame is the real local level navigation coordinate frame; there is some error between  and   frame owing to the alignment error and the sensor error.
(iii) The  frame is the strapdown inertial sensor's body coordinate frame.
(iv) The  frame is the Earth coordinate frame.
(v) The  frame is the nonrotating inertial coordinate frame.
The common navigation (attitude, velocity, and position) update equations are written in the  frame as [12][13][14][15] where    is the attitude direction cosine matrix (DCM) from the  frame to the  frame, v  is the velocity in the  frame, f  is the special force measured by the accelerometers fixed in the  frame,     is the angular rate of the Earth's rotation in the  frame,     is the angular rate caused by the velocity on the Earth (since the Earth is nearly round) in the  frame, g  is the gravity in the  frame, (⋅)× is the matrix form of a cross-product which satisfies a × b = (a×)b, p is the position described in the  frame as p = [, , ℎ]  ,  is the longitude,  is the latitude, ℎ is the height above the mean sea level,  is the radius of the Earth,     is the angular rate measured by the gyroscopes in the  frame,    is the angular rate of the SINS in the  frame with respect to the  frame described as and R  is the velocity transformation matrix between the  frame and the  frame defined as The velocity v  can be measured by the DVL if    is known.The purpose of the alignment is to determine the attitude DCM    and the position p based on the data collected from the accelerometers, the gyroscopes, and the DVL while in motion.

Attitude Alignment
3.1.Attitude Coarse Alignment.Since the velocity of the DVL is in the  frame, the aim here is to convert (2) into the  frame too.Equation ( 2) can be written as Substituting (1) into (6) yields This is the complete form of the velocity update equation in the  frame.In fact,    (2   +    ) × (   k  ) 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 Equation ( 1) is the traditional attitude update equation.Nowadays, the attitude update is separated using the DCM product chain rule as [12]  (+Δ) (+Δ) =  (+Δ) () The update method for  (+Δ) () and  () (+Δ) is also proposed in [12].If this rule is applied in each update cycle, the attitude update equation will be The update equations for  (0) () and  (0) () are [11] Ċ (0) () =  (0) ()    ×, where    =    +    ≈    for the reason that    is unknown but minor in alignment stage.If  (0) (0) is identified,  () () on every time step is known.The procedure identifying  (0) (0) is as follows.Substitute (10) into (8) and multiply  (0) () on each part Integrating ( 13) on both sides where The initial attitude matrix  (0) (0) can be uniquely solved by the optimization-based method described in [16,17] to minimize So the key is how to calculate () and () with different integral time.

Integration Strategy.
If k  ,    , and f  were known, (13) could be the model for calculating  (0) (0) .This cannot be achieved by real sensors.The DVL's output is the velocity in the  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 Substitute ( 4) and ( 12) into ( 18) and omit the  (0) ()  () () (   +    ) × k  part for the same reason as the AUVs would not voyage in a very high speed The calculation methods for the other part of ( 15) and the whole equation ( 16) are already expressed in [11] as So where the angular rate and the specific force are assumed in linear forms within a short time as The c  , d  , c  , and d  are the coefficient vectors as where  is the time period of one update interval [   +1 ] ,  = 0, 1, 2, . . .,  − 1, with   = , and Δv 1 , Δv 2 and Δ 1 , Δ 2 are the incremental velocity and angle collected from the accelerometers and gyroscopes.

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 (), () should be suitable for these conditions.
For () 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 () expressed in (21), the update rate is the same as the gyroscopes and accelerometers.And the output rate is kept the same as () since the algorithm needs the data at the same time point.Consider where  = 0, 1, 2, . . .,   means the time point that the DVL is available.The least number of  is decided by the sensor error level.
Owing to the low and unsteady output rate of the DVL, the v  (0) is almost not obtainable.This part can be counteracted as This is the practical equation for the coarse alignment.The goal is to improve the attitude accuracy especially the heading accuracy.

Attitude Fine
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 And the large misalignment angles model is where ŷ is the real data equal to the ideal data  plus the error ,     is the DCM from the  frame to the   frame,   and   are the noise of the gyroscopes and accelerometers, and is the DCM from the  frame to the   frame.Assume that the  frame to the   frame is completed through three noninterchangeable rotations   ,   , and   .Each rotation can be expressed as The total DCM is The measurement model is where k  DVL is the noise of the DVL.Then according to (29) and (32), the nonlinear state and measurement equations are where w = [0 3×1 0 3×1     ] is the state noise and u = [−k  DVL ] is the measurement noise.
The fine alignment's duty is to estimate     to improve the accuracy of    .Notice that  (0) (0) ,  (0) () , and  (0) () can still be updated in fine alignment.It means that a more precise  (0) (0) is realized too according to the inverse of (10) in the fine alignment stage as This result is of great importance in the position alignment.

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  frame and the  frame as In order to get the position, (35) should be integrated on each side first.The integral form of the left term is where  end is the time point where the attitude alignment ends.Substitute (11) into (36): The integral part of (37) can be converted as where   =  end , when  = .Practically, the effort of the position change is really small in the integration.It could be regarded as constant as where p( end ) is the position which needs alignment.Substitute (39) into the integral part of (38): Therefore the computed form of (36) is The integral form of the right term of ( 35) is where   =  end , when  = .Similarly, v  can be approximated linearly as where  DVL is the duration of two DVL samples.Substitute (23), (25), and (44) into (43): According to (35), (42) equals (45).It follows that For ( 46), all the data can be calculated in real-time except  (0) (0) .It means that when the initial attitude matrix  (0) is obtained, the position could immediately be updated.As described in Section 3,  (0) (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 And then, (41) goes to And by making (48) be equal to (45), another solution comes out as (49) And we can also let (41) be equal to (45) to get another solution as 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.

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 3. Update the whole equation of (22) when the DVL's data is available.
Step 6. Go to Step 2 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 10.Update the position via (46).Step 11.Go to Step 7 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.

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.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.
Figure 2 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  frame.And for the real sensors, the sensor errors would affect the accuracy much more than the omitted parts.
Figure 3 illustrates the alignment errors with sensor errors (gyroscopes drift: 0.2 deg/h, gyroscopes noise:   a significant increase and decrease process in the first 20 s; this is mainly because the accuracy of the initial attitude DCM  (0) (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.
Figure 5 demonstrates the average position errors with sensor errors.The position errors significantly increase.Still the largest error source is the sensor errors.

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 Figure 6.The sensors used for the experiment are listed below.
(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.
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.

In-Motion Alignment Results
. The trajectory of the towed body is shown in Figure 7. 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.
The attitude alignment results are presented in Figures 8,  9 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.Figures 13 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.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.

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 the  frame and position in the  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.

Figure 1 :
Figure 1: Average attitude errors of four types of trajectories.

Figure 2 :Figure 3 :
Figure 2: Average heading errors of Tr4 and Tr4 with the omitted parts kept (Tr4 no).

Figure 4 :
Figure 4: Average position errors of four types of trajectories with three different position approximate methods.

Figure 4
Figure 4 displays the average position errors of these three different approximations.The simulation still runs 50 times with four types of trajectories.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

Figure 5 :Figure 6 :
Figure 5: Average position errors of Tr3 with Ap1 and Ap1 with sensor errors (Ap1 SE).Dashed lines are the 3 envelopes.

Figure 7 :
Figure 7: The route of the towed body.

Figure 8 :Figure 9 :
Figure 8: The heading error of the alignment.(a) Overview.(b) Details of the coarse alignment.(c) Details of the fine alignment.

1 Figure 13 :Figure 14 :
Figure 13: The comparison of the heading error of IMA1 and IMA2.
Alignment.According to the sensor error level, about 200 samples ( ≈ 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.