A Fast in-Motion Alignment Algorithm for DVL Aided SINS

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.

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

2. 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–15] 𝐶̇^𝑛_𝑏=𝐶^𝑛_𝑏𝝎^𝑏_𝑛𝑏×,𝐯̇^𝑛=𝐶^𝑛_𝑏𝐟^𝑏−2𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛×𝐯^𝑛+𝐠^𝑛,𝐩̇=𝐑_𝑐𝐯^𝑛, (1)(2)(3) where 𝐶^𝑛_𝑏 is the attitude direction cosine matrix (DCM) from the 𝑏 frame to the 𝑛 frame, 𝐯^𝑛 is the velocity in the 𝑛 frame, 𝐟^𝑏 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, 𝐠^𝑛 is the gravity in the 𝑛 frame, (·)× is the matrix form of a cross-product which satisfies 𝐚×𝐛=(𝐚×)𝐛,𝐩 is the position described in the 𝑒 frame as 𝐩=[𝜆,𝜑,ℎ]^𝑇, 𝜆 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 𝝎^𝑏_𝑛𝑏=𝝎^𝑏_𝑖𝑏−𝐶^𝑏_𝑛𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛, (4) and 𝐑_𝑐 is the velocity transformation matrix between the 𝑛 frame and the 𝑒 frame defined as 𝐑_𝑐=sec𝜑01𝑅+ℎ000.𝑅+ℎ001 (5)

The velocity 𝐯^𝑛 can be measured by the DVL if 𝐶^𝑛_𝑏 is known. The purpose of the alignment is to determine the attitude DCM 𝐶^𝑛_𝑏 and the position 𝐩 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 the 𝑏 frame, the aim here is to convert (2) into the 𝑏 frame too. Equation (2) can be written as 𝐯̇^𝑛=𝑑𝐶^𝑛_𝑏𝐯^𝑏=𝐶̇𝑑𝑡^𝑛𝑏𝐯^𝑏+𝐶^𝑛_𝑏𝐯̇^𝑏=𝐶^𝑛_𝑏𝐟^𝑏−2𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛×𝐯^𝑛+𝐠^𝑛. (6) Substituting (1) into (6) yields 𝐯̇^𝑏=𝐟^𝑏−𝐶^𝑏_𝑛2𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛×𝐶^𝑛_𝑏𝐯^𝑏−𝝎^𝑏_𝑛𝑏×𝐯^𝑏+𝐶^𝑏_𝑛𝐠^𝑛. (7) This is the complete form of the velocity update equation in the 𝑏 frame. In fact, 𝐶^𝑏_𝑛(2𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛)×(𝐶^𝑛_𝑏𝐯^𝑏) 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) Equation (1) is the traditional attitude update equation. Nowadays, the attitude update is separated using the DCM product chain rule as [12] 𝐶_𝑏()^{𝑛(𝑡+Δ𝑡)}_𝑡+Δ𝑡=𝐶_𝑛(𝑡)^{𝑛(𝑡+Δ𝑡)}𝐶_𝑏(𝑡)^𝑛(𝑡)𝐶_𝑏()^𝑏(𝑡)_𝑡+Δ𝑡. (9) 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 𝐶_𝑏(𝑡)^𝑛(𝑡)=𝐶_𝑛(0)^𝑛(𝑡)𝐶_𝑏(0)^𝑛(0)𝐶_𝑏(𝑡)^𝑏(0). (10) The update equations for 𝐶^𝑛(0)_𝑛(𝑡) and 𝐶^𝑏(0)_𝑏(𝑡) are [11] 𝐶̇_𝑛(𝑡)^𝑛(0)=𝐶_𝑛(𝑡)^𝑛(0)𝝎^𝑛_𝑖𝑛×,𝐶̇_𝑏(𝑡)^𝑏(0)=𝐶_𝑏(𝑡)^𝑏(0)𝝎^𝑏_𝑖𝑏×, (11)(12) 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 𝐶_𝑏(𝑡)^𝑏(0)𝐯̇^𝑏−𝐶_𝑏(𝑡)^𝑏(0)𝐟^𝑏+𝐶_𝑏(𝑡)^𝑏(0)𝝎^𝑏_𝑛𝑏×𝐯^𝑏=𝐶_𝑛(0)^𝑏(0)𝐶_𝑛(𝑡)^𝑛(0)𝐠^𝑛. (13) Integrating (13) on both sides 𝜶(𝑡)=𝐶_𝑛(0)^𝑏(0)𝜷(𝑡), (14) where 𝜶(𝑡)=^𝑡₀𝐶_𝑏(𝑡)^𝑏(0)𝐯̇^𝑏+𝐶_𝑏(𝑡)^𝑏(0)𝝎^𝑏_𝑛𝑏×𝐯^𝑏−𝐶_𝑏(𝑡)^𝑏(0)𝐟^𝑏𝑑𝑡,𝜷(𝑡)=^𝑡₀𝐶_𝑛(𝑡)^𝑛(0)𝐠^𝑛𝑑𝑡. (15)(16) The initial attitude matrix 𝐶^𝑛(0)_𝑏(0) can be uniquely solved by the optimization-based method described in [16, 17] to minimize 𝐿𝐶_𝑛(0)^𝑏(0)=𝜶𝑡_𝑁−𝐶_𝑛(0)^𝑏(0)𝜷𝑡_𝑁². (17) So the key is how to calculate 𝜶(𝑡) and 𝜷(𝑡) with different integral time.

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

Calculation scheme for 𝜶(𝑡), 𝜷(𝑡) includes two parts: integration strategy and update scheme.

3.2.1. Integration Strategy

If 𝐯̇^𝑏, 𝝎^𝑏_𝑖𝑏, and 𝐟^𝑏 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 ^𝑡₀𝐶_𝑏(𝑡)^𝑏(0)𝐯̇^𝑏+𝐶_𝑏(𝑡)^𝑏(0)𝝎^𝑏_𝑛𝑏×𝐯^𝑏𝑑𝑡=𝐶_𝑏(𝑡)^𝑏(0)𝐯^𝑏𝑡₀−^𝑡₀𝐶̇_𝑏(𝑡)^𝑏(0)𝐯^𝑏−𝐶_𝑏(𝑡)^𝑏(0)𝝎^𝑏_𝑛𝑏×𝐯^𝑏𝑑𝑡. (18) Substitute (4) and (12) into (18) and omit the 𝐶^𝑏(0)_𝑏(𝑡)𝐶^𝑏(𝑡)_𝑛(𝑡)(𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛)×𝐯^𝑏 part for the same reason as the AUVs would not voyage in a very high speed ^𝑡₀𝐶_𝑏(𝑡)^𝑏(0)𝐯̇^𝑏+𝐶_𝑏(𝑡)^𝑏(0)𝝎^𝑏_𝑛𝑏×𝐯^𝑏𝑑𝑡≈𝐶_𝑏(𝑡)^𝑏(0)𝐯^𝑏𝑡₀. (19)

The calculation methods for the other part of (15) and the whole equation (16) are already expressed in [11] as ^𝑡₀𝐶_𝑏(𝑡)^𝑏(0)𝐟^𝑏𝑑𝑡=^𝑁−1_𝑘=0𝐶_𝑏𝑡^𝑏(0)_𝑘^𝑡𝑘+1_𝑡𝑘𝐶^{𝑏(𝑡𝑘)}_𝑏(𝑡)𝐟^𝑏≈𝑑𝑡_{𝑁−1𝑘=0}𝐶_𝑏𝑡^𝑏(0)_𝑘^𝑡𝑘+1_𝑡𝑘𝐈+^𝑡_𝑡𝑘𝝎^𝑏_𝑖𝑏×𝑑𝜏𝐟^𝑏=𝑑𝑡_{𝑁−1𝑘=0}𝐶_𝑏𝑡^𝑏(0)_𝑘Δ𝐯₁+Δ𝐯₂+12Δ𝜽₁+Δ𝜽₂×Δ𝐯₁+Δ𝐯₂+23Δ𝜽₁×Δ𝐯₂+Δ𝐯₁×Δ𝜽₂,𝜷𝑡_𝑁=^𝑡₀𝐶_𝑛(𝑡)^𝑛(0)𝐠^𝑛𝑑𝑡=^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘^𝑡𝑘+1_𝑡𝑘𝐶^{𝑛(𝑡𝑘)}_𝑛(𝑡)𝐠^𝑛≈𝑑𝑡_{𝑁−1𝑘=0}𝐶_𝑛𝑡^𝑛(0)_𝑘^𝑡𝑘+1_𝑡𝑘𝐈+𝑡−𝑡_𝑘𝜔^𝑛_𝑖𝑛×𝐠^𝑛=𝑑𝑡_{𝑁−1𝑘=0}𝐶_𝑛𝑡^𝑛(0)_𝑘𝑇𝑇𝐈+²2𝜔^𝑛_𝑖𝑛×𝐠^𝑛≈^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝑇𝑇𝐈+²2𝜔^𝑛_𝑖𝑒×𝐠^𝑛. (20)(21) So 𝜶𝑡_𝑁≈𝐶_𝑏𝑡^𝑏(0)_𝑁𝐯^𝑏𝑡_𝑁−𝐯^𝑏(0)−^𝑁−1_𝑘=0𝐶_𝑏𝑡^𝑏(0)_𝑘Δ𝐯₁+Δ𝐯₂+12Δ𝜽₁+Δ𝜽₂×Δ𝐯₁+Δ𝐯₂+23Δ𝜽₁×Δ𝐯₂+Δ𝐯₁×Δ𝜽₂, (22) where the angular rate and the specific force are assumed in linear forms within a short time as 𝝎^𝑏_𝑖𝑏=𝑡−𝑡_𝑘𝐜_𝜔+𝐝_𝜔,𝐟^𝑏=𝑡−𝑡_𝑘𝐜_𝑓+𝐝_𝑓. (23) The 𝐜_𝑓, 𝐝_𝑓, 𝐜_𝜔, and 𝐝_𝜔 are the coefficient vectors as 𝐜_𝑓=4Δ𝐯₂−Δ𝐯₁𝑇²,𝐝_𝑓=3Δ𝐯₁−Δ𝐯₂𝑇,𝐜_𝜔=4Δ𝜽₂−Δ𝜽₁𝑇²,𝐝_𝜔=3Δ𝜽₁−Δ𝜽₂𝑇, (24)(25) where 𝑇 is the time period of one update interval [𝑡_𝑘𝑡_𝑘+1], 𝑘=0,1,2,…,𝑁−1, with 𝑡_𝑘=𝑘𝑇, and Δ𝐯₁, Δ𝐯₂ and Δ𝜽₁, Δ𝜽₂ 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 𝜶(𝑡), 𝜷(𝑡) 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 𝜶(𝑡)⟶𝜶_DVL𝑡_𝑀,𝜷(𝑡)⟶𝜷_DVL𝑡_𝑀, (26) 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 𝐯^𝑏(0) is almost not obtainable. This part can be counteracted as 𝜶_DVL𝑡_𝑀−𝜶_DVL𝑡₀=𝐶_𝑛(0)^𝑏(0)𝜷_DVL𝑡_𝑀−𝜷_DVL𝑡₀. (27) This is the practical equation for the coarse alignment.

3.3. Attitude Fine 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. 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) And the large misalignment angles model is 𝛿𝐯̇^𝑛=𝐈−𝐶^𝑛_𝑛′𝐶^𝑛′_𝑏𝐟^𝑏̂−2𝛿𝝎^𝑛_𝑖𝑒+𝛿𝝎^𝑛_𝑒𝑛×𝐯^𝑛−2𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛×𝛿𝐯^𝑛+𝐶^𝑛_𝑛′𝐶^𝑛′_𝑏𝛿𝐟^𝑏,𝜽̇=𝐈−𝐶^𝑛′_𝑛𝝎^𝑛_𝑖𝑛+𝛿𝝎^𝑛_𝑖𝑛−𝐶^𝑛′_𝑏𝛿𝝎^𝑏_𝑖𝑏,𝛿𝝎̇^𝑏_𝑖𝑏=𝟎+𝜺_𝜔,𝛿𝐟^𝑏̇=𝟎+𝜺_𝑓, (29) 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 𝐶_𝜃𝑥=1000cos𝜃_𝑥sin𝜃_𝑥0−sin𝜃_𝑥cos𝜃_𝑥,𝐶_𝜃𝑦=cos𝜃_𝑦0−sin𝜃_𝑦010sin𝜃_𝑦0cos𝜃_𝑦,𝐶_𝜃𝑧=cos𝜃_𝑧sin𝜃_𝑧0−sin𝜃_𝑧cos𝜃_𝑧0.001 (30) The total DCM is 𝐶^𝑛′_𝑛=𝐶_𝜃𝑦𝐶_𝜃𝑥𝐶_𝜃𝑧. (31)

The measurement model is 𝐳=𝐯^𝑛−𝐶^𝑛_𝑛′𝐶^𝑛′_𝑏𝐯^𝑏_DVL=𝐯^𝑛+𝛿𝐯^𝑛−𝐶^𝑛_𝑛′𝐶^𝑛′_𝑏𝐯^𝑏_DVL+𝛿𝐯^𝑏_DVL=𝐯^𝑛−𝐯^𝑛_DVL+𝛿𝐯^𝑛−𝛿𝐯^𝑛_DVL=𝛿𝐯^𝑛−𝛿𝐯^𝑛_DVL, (32) where 𝛿𝐯^𝑛_DVL is the noise of the DVL.

Then according to (29) and (32), the nonlinear state and measurement equations arė𝛿𝐯̇𝐱=^𝑛𝜽̇𝛿𝝎̇^𝑏_𝑖𝑏𝛿𝐟^𝑏̇(𝐱)==𝑓+𝐰𝐈−𝐶^𝑛_𝑛′𝐶^𝑛′_𝑏𝐟^𝑏̂−2𝛿𝝎^𝑛_𝑖𝑒+𝛿𝝎^𝑛_𝑒𝑛×𝐯^𝑛−2𝝎^𝑛_𝑖𝑒+𝝎^𝑛_𝑒𝑛×𝛿𝐯^𝑛+𝐶^𝑛_𝑛′𝐶^𝑛′_𝑏𝛿𝐟^𝑏𝐈−𝐶^𝑛′_𝑛𝝎^𝑛_𝑖𝑛+𝛿𝝎^𝑛_𝑖𝑛−𝐶^𝑛′_𝑏𝛿𝝎^𝑏_𝑖𝑏00+00𝜺_𝜔𝜺_𝑓,𝐳=ℎ(𝐱)+𝐮=𝛿𝐯^𝑛+−𝛿𝐯^𝑛_DVL, (33)where 𝐰=𝟎_3×1𝟎_3×1𝜺_𝜔𝜺_𝑓 is the state noise and 𝐮=[−𝛿𝐯^𝑛_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 𝐶_𝑏(0)^𝑛(0)=𝐶_𝑛(𝑡)^𝑛(0)𝐶_𝑏(𝑡)^𝑛(𝑡)𝐶_𝑏(0)^𝑏(𝑡). (34) 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 𝑒 frame and the 𝑏 frame as 𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩̇(𝑡)=𝐶_𝑏(0)^𝑛(0)𝐶_𝑏(𝑡)^𝑏(0)𝐯^𝑏(𝑡). (35) In order to get the position, (35) should be integrated on each side first. The integral form of the left term is ^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩̇(𝑡)𝑑𝑡=𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩(𝑡)^𝑡end₀−^𝑡end₀𝐶̇_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩(𝑡)𝑑𝑡, (36) where 𝑡_end is the time point where the attitude alignment ends. Substitute (11) into (36): ^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩̇(𝑡)𝑑𝑡=𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩(𝑡)^𝑡end₀−^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡)𝑑𝑡. (37) The integral part of (37) can be converted as ^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡)=𝑑𝑡_{𝑁−1𝑘=0}𝐶_𝑛𝑡^𝑛(0)_𝑘^𝑡𝑘+1_𝑡𝑘𝐶^{𝑛(𝑡𝑘)}_𝑛(𝑡)𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡)𝑑𝑡, (38) where 𝑡_𝑘=𝑡_end, when 𝑘=𝑁. Practically, the effort of the position change is really small in the integration. It could be regarded as constant as 𝐩(𝑡)=𝐩𝑡_end, (39) where 𝐩(𝑡_end) is the position which needs alignment. Substitute (39) into the integral part of (38): ^𝑡𝑘+1_𝑡𝑘𝐶^{𝑛(𝑡𝑘)}_𝑛(𝑡)𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡)≈𝑑𝑡_{𝑡𝑘+1𝑡𝑘}𝐈+𝑡−𝑡_𝑘𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑑𝑡=𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑇+𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑇²2. (40) Therefore the computed form of (36) is ^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩̇𝑑𝑡≈𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end−𝐑_𝑐⁻¹𝐩(0)−^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑇+𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑇²2. (41) For (41), 𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡_end)𝑇²/2, which is a second-order term, is much smaller than 𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡_end)𝑇 with 𝑇<1. So this part could be omitted. Then, notice that 𝑇𝝎^𝑛_𝑖𝑛×≈𝐂^{𝑛(𝑡𝑘)}_{𝑛(𝑡𝑘+1)}−𝐈; (41) can be simplified as ^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩̇𝑑𝑡≈𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end−𝐑_𝑐⁻¹𝐩(0)−^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝐂^{𝑛(𝑡𝑘)}_{𝑛𝑡𝑘+1}−𝐈𝐑_𝑐⁻¹𝐩𝑡_end≈𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end−𝐑_𝑐⁻¹𝐩(0)−𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end−𝐑_𝑐⁻¹𝐩𝑡_end=𝐑_𝑐⁻¹𝐩𝑡_end−𝐑_𝑐⁻¹𝐩(0). (42) The integral form of the right term of (35) is ^𝑡end₀𝐶_𝑏(0)^𝑛(0)𝐶_𝑏(𝑡)^𝑏(0)𝐯^𝑏𝑑𝑡=𝐶_𝑏(0)^{𝑛(0)𝑀−1}_𝑗=0𝐶_𝑏𝑡^𝑏(0)_𝑗^𝑡𝑗+1_𝑡𝑗𝐶^{𝑏(𝑡𝑗)}_𝑏(𝑡)𝐯^𝑏𝑑𝑡≈𝐶_𝑏(0)^{𝑛(0)𝑀−1}_𝑗=0𝐶_𝑏𝑡^𝑏(0)_𝑗^𝑡𝑗+1_𝑡𝑗𝐈+^𝑡_𝑡𝑗𝝎^𝑏_𝑖𝑏𝑑𝜏×𝐯^𝑏𝑑𝑡, (43) where 𝑡_𝑗=𝑡_end, when 𝑗=𝑀. Similarly, 𝐯^𝑏 can be approximated linearly as 𝐯^𝑏(𝑡)=𝐯^𝑏𝑡_𝑗+𝑡−𝑡_𝑗𝑇_DVL𝐯^𝑏𝑡_𝑗+1−𝐯^𝑏𝑡_𝑗, (44) where 𝑇_DVL is the duration of two DVL samples. Substitute (23), (25), and (44) into (43): ^𝑡end₀𝐶_𝑏(0)^𝑛(0)𝐶_𝑏(𝑡)^𝑏(0)𝐯^𝑏𝑑𝑡≈𝐶_𝑏(0)^{𝑛(0)𝑀−1}_𝑗=0𝐶_𝑏𝑡^𝑏(0)_𝑗𝑇_DVL2𝐯^𝑏𝑡_𝑘+1+𝐯^𝑏𝑡_𝑘+13Δ𝜽₁×𝑇_DVL𝐯^𝑏𝑡_𝑘+12Δ𝜽₁+16Δ𝜽₂×𝑇_DVL𝐯^𝑏𝑡_𝑘+1. (45) According to (35), (42) equals (45). It follows that 𝐩𝑡_end=𝐩(0)+𝐑_𝑐𝐶_𝑏(0)^{𝑛(0)𝑀−1}_𝑗=0𝐶_𝑏𝑡^𝑏(0)_𝑗𝑇_DVL2𝐯^𝑏𝑡_𝑘+1+𝐯^𝑏𝑡_𝑘+13Δ𝜽₁×𝑇_DVL𝐯^𝑏𝑡_𝑘+12Δ𝜽₁+16Δ𝜽₂×𝑇_DVL𝐯^𝑏𝑡_𝑘+1. (46)

For (46), all the data can be calculated in real-time except 𝐶^𝑛(0)_𝑏(0). It means that when the initial attitude matrix 𝐶^𝑛(0)_𝑏(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 𝐩(𝑡)=𝐩(0). (47) And then, (41) goes to ^𝑡end₀𝐶_𝑛(𝑡)^𝑛(0)𝐑_𝑐⁻¹𝐩̇𝑑𝑡≈𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end−𝐑_𝑐⁻¹𝐩(0)−^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(0)𝑇+𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(0)𝑇²2. (48) And by making (48) be equal to (45), another solution comes out as 𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end≈𝐶_𝑏(0)^{𝑛(0)𝑁−1}_𝑘=0𝐶_𝑏𝑡^𝑏(0)_𝑘𝑇_DVL2𝐯^𝑏𝑡_𝑘+1+𝐯^𝑏𝑡_𝑘+13Δ𝜽₁×𝑇_DVL𝐯^𝑏𝑡_𝑘+12Δ𝜽₁+16Δ𝜽₂×𝑇_DVL𝐯^𝑏𝑡_𝑘+1+𝐑_𝑐⁻¹𝐩(0)+^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(0)𝑇+^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(0)𝑇²2. (49) And we can also let (41) be equal to (45) to get another solution as 𝐶_𝑛𝑡^𝑛(0)_end𝐑_𝑐⁻¹𝐩𝑡_end≈𝐶_𝑏(0)^{𝑛(0)𝑁−1}_𝑘=0𝐶_𝑏𝑡^𝑏(0)_𝑘𝑇_DVL2𝐯^𝑏𝑡_𝑘+1+𝐯^𝑏𝑡_𝑘+13Δ𝜽₁×𝑇_DVL𝐯^𝑏𝑡_𝑘+12Δ𝜽₁+16Δ𝜽₂×𝑇_DVL𝐯^𝑏𝑡_𝑘+1+𝐑_𝑐⁻¹𝐩(0)+^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑇+^𝑁−1_𝑘=0𝐶_𝑛𝑡^𝑛(0)_𝑘𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩𝑡_end𝑇²2. (50) 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. Set 𝐶^𝑛(0)_𝑛(0)=𝐶^𝑏(0)_𝑏(0)=𝐈, 𝐩(0) as the beginning position coordinate.

Step 2. Update 𝐶^𝑛(0)_𝑛(𝑡), 𝐶^𝑏(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 matrix 𝐶^𝑛(0)_𝑏(0) based on (27). Compute the attitude matrix 𝐶^𝑛(𝑡)_𝑏(𝑡) by (10).

Step 5. Update the position via (46).

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 8. Update 𝐶^𝑛(0)_𝑛(𝑡), 𝐶^𝑏(0)_𝑏(𝑡) when the data of inertial sensors is available.

Step 9. Compute a more accurate 𝐶^𝑛(0)_𝑏(0) according to (34).

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.

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. Figure 1 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.

(a)

(b)

(c)

Figure 1 

Average attitude errors of four types of trajectories.

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 2 

Average heading errors of Tr4 and Tr4 with the omitted parts kept (Tr4_no).

Figure 3 illustrates the alignment errors with sensor errors (gyroscopes drift: 0.2 deg/h, gyroscopes noise: 0.9 deg/h/√Hz, accelerometers bias: 1×10⁻⁴ g, accelerometers noise: 2×10⁻⁴ 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.

(a)  

(b)

(c)

Figure 3 

Average attitude errors of Tr3 and Tr3 with sensor errors (Tr3_SE). Dashed lines are the 3𝜎 envelopes.

For the position alignment, three different position approximate methods are applied in Section 4. Approximation 1 (Ap1): regard 𝐩(𝑡) as 𝐩(𝑡_end), and 𝝎^𝑛_𝑖𝑛×𝝎^𝑛_𝑖𝑛×𝐑_𝑐⁻¹𝐩(𝑡_end)𝑇²/2 part is omitted. Approximation 2 (Ap 2): regard 𝐩(𝑡) as 𝐩(0). Approximation 3 (Ap 3): regard 𝐩(𝑡) as 𝐩(𝑡_end).

Figure 4 displays the average position errors of these three different approximations. The simulation still runs 50 times with four types of trajectories.

(a)  

(b)

(c)

(d)

Figure 4 

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

As shown in Figure 4, 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 𝐶^𝑛(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.

Figure 5 

Average position errors of Tr3 with Ap1 and Ap1 with sensor errors (Ap1_SE). Dashed lines are the 3𝜎 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 Figure 6. 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⁻⁴ 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 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.

Figure 7 

The route of the towed body.

The attitude alignment results are presented in Figures 8, 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.

(a)

(b)

(c)

Figure 8 

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

Figure 9 

The pitch error of the alignment.

Figure 10 

The roll error of the alignments.

The position alignment results are presented in Figures 11 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.

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.

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

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgment

This work was supported by The Principal Fund of Zhejiang University.