Research Article A Fast in-Motion Alignment Algorithm for...
13
Research Article A Fast in-Motion Alignment Algorithm for DVL Aided SINS Li Kang, Lingyun Ye, and Kaichen Song College of Biomedical Engineering & Instrument Science, Zhejiang University, Hangzhou, Zhejiang 310027, China Correspondence should be addressed to Lingyun Ye; [email protected] Received 3 March 2014; Revised 12 May 2014; Accepted 12 May 2014; Published 4 June 2014 Academic Editor: Chia-Cheng Tsai Copyright © 2014 Li Kang et al. is is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. 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. is paper proposed a complete in-motion alignment solution for both attitude and position. e 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. e 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. e 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. e final position error could achieve 1.5% of the voyage distance. is 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. e first is that the alignment needs to finish in-motion since the vehicle cannot keep stationary due to the water flow. e 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. e attitude alignment oſten includes two stages: coarse alignment and fine alignment. e 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. is 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]. ese 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]. is 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. e magnetic compass is not robust in such harsh environment. e task of the position alignment is to provide the position during the attitude alignment process. is is mainly provided by the GPS for on-land assignments [11], while Hindawi Publishing Corporation Mathematical Problems in Engineering Volume 2014, Article ID 593692, 12 pages http://dx.doi.org/10.1155/2014/593692
Transcript of Research Article A Fast in-Motion Alignment Algorithm for...
Research ArticleA Fast in-Motion Alignment Algorithm for DVL Aided SINS
Li Kang Lingyun Ye and Kaichen Song
College of Biomedical Engineering amp Instrument Science Zhejiang University Hangzhou Zhejiang 310027 China
Correspondence should be addressed to Lingyun Ye lyyezjueducn
Received 3 March 2014 Revised 12 May 2014 Accepted 12 May 2014 Published 4 June 2014
Academic Editor Chia-Cheng Tsai
Copyright copy 2014 Li Kang et al This is an open access article distributed under the Creative Commons Attribution License whichpermits unrestricted use distribution and reproduction in any medium provided the original work is properly cited
Doppler velocity log (DVL) aided strapdown inertial navigation system (SINS) is a common navigation method for underwaterapplications Owing to the in-motion condition and the lack of the GPS it is a challenge to align a SINS under water This paperproposed a complete in-motion alignment solution for both attitude and position The velocity update equation and its integralform in the body frame are studied and the attitude coarse alignment becomes an optimization-based attitude determinationproblem between the body frame velocity and the integral form of gravity The body frame velocity and the Earth frame positionare separately treated and the position alignment problem turns into an equation solving problem Simulation and on-lake tests arecarried out to examine the algorithm The heading could reach around 10 deg accuracy and the pitch and roll could be aligned upto 005 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 15 of the voyage distance This scheme can also be applied toother body frame velocity aided SINS alignments
1 Introduction
DVL aided SINS is now widely used in the autonomousunderwater vehicles (AUVs) [1ndash3] It is a challenge to aligna SINS under water for two reasons The first is that thealignment needs to finish in-motion since the vehicle cannotkeep stationary due to the water flow The second is thatthe GPS [4 5] the most useful aided sensor which couldprovide geodetic frame velocity and the Earth frame positionis not available under water So it is necessary to developDVL aided in-motion alignment Due to the lack of positioninformation the alignment should include both attitude andposition
The attitude alignment often includes two stages coarsealignment and fine alignment The attitude fine alignment ismainly based on the Earth frame position or geodetic velocitymatching with Kalman filter (KF) [6] Currently the bodyframe velocity matching alignment could already partly solvethe problem if a roughly known attitude could pass to thefilter But if the error of the roughly known attitude is toolarge the alignment would take much time and the finalresults would be worse [7] So the key process is to get this
roughly known attitude This process is called the attitudecoarse alignment
Various algorithms for the coarse alignment have beendeveloped One kind of algorithms determined the attitudefrom gyroscope and accelerometer measurements using thegravity and the Earthrsquos rotation as a reference [6 8 9] Thesealgorithms are fit for the condition that the SINS is stationaryor quasi-stationary While in-motion condition the angularrate is much larger than the Earthrsquos rotation the attitudeerror is large in most situations Another kind of algorithmsapplied the velocity of the GPS as an input and optimizationapproach is used between the velocities of the navigationframe and the body frame [10 11] This method could offera good result to the fine alignment in in-motion conditionBut there is no GPS signal under water Also it is not a goodidea to get the initial attitude using amagnetic compass sincethere are many electromagnetic interference sources such asthe motors and the solenoid for the emergency jettison Themagnetic compass is not robust in such harsh environment
The task of the position alignment is to provide theposition during the attitude alignment processThis is mainlyprovided by the GPS for on-land assignments [11] while
Hindawi Publishing CorporationMathematical Problems in EngineeringVolume 2014 Article ID 593692 12 pageshttpdxdoiorg1011552014593692
2 Mathematical Problems in Engineering
the GPS is not available for underwater assignments Themain idea is to use the DVL to reckon the coordinate fromthe start point The problem is that the body frame velocitycannot be integrated into position before the attitude isknown
The contents are organized as follows Section 2 explainsthe problem in mathematics Section 3 is the attitude align-ment section The attitude coarse alignment is devised byfurther study of the velocity update equation in the nav-igation frame and its transformation in the body frameOptimization approach is applied between the body framevelocity and the integral formof the gravity Also the commonlarge misalignment angles model based UKF is reviewedin Section 3 Section 4 proposed the position alignment byseparately treating the body frame velocity and the Earthframe position And when the initial attitude is gathered theposition could be straight obtained through an equationTheperformance of the algorithm is evaluated with simulationin Section 5 and on-lake tests in Section 6 Conclusions aredrawn in Section 7
2 Problem Statement
The coordinate frames used for the in-motion alignment aredefined as follows
(i) The 119899 frame is the ideal local level navigation coordi-nate frame with east-north-up geodetic axes
(ii) The 1198991015840 frame is the real local level navigation coordi-nate frame there is some error between 119899 and 1198991015840 frameowing to the alignment error and the sensor error
(iii) The 119887 frame is the strapdown inertial sensorrsquos bodycoordinate frame
(iv) The 119890 frame is the Earth coordinate frame(v) The 119894 frame is the nonrotating inertial coordinate
frame
The common navigation (attitude velocity and position)update equations are written in the 119899 frame as [12ndash15]
119899
119887= 119862119899
119887120596119887
119899119887times (1)
k119899 = 119862119899
119887f119887 minus (2120596
119899
119894119890+ 120596119899
119890119899) times k119899 + g119899 (2)
p = R119888k119899 (3)
where 119862119899119887is the attitude direction cosine matrix (DCM) from
the 119887 frame to the 119899 frame v119899 is the velocity in the 119899 framef119887 is the special force measured by the accelerometers fixedin the 119887 frame 120596119899
119894119890is the angular rate of the Earthrsquos rotation
in the 119899 frame 120596119899119890119899is the angular rate caused by the velocity
on the Earth (since the Earth is nearly round) in the 119899 frameg119899 is the gravity in the 119899 frame (sdot)times is the matrix form of across-product which satisfies a times b = (atimes)b p is the positiondescribed in the 119890 frame as p = [120582 120593 ℎ]
119879 120582 is the longitude120593 is the latitude ℎ is the height above the mean sea level 119877is the radius of the Earth 120596119887
119894119887is the angular rate measured by
the gyroscopes in the 119887 frame 120596119887119899119887
is the angular rate of theSINS in the 119887 frame with respect to the 119899 frame described as
120596119887
119899119887= 120596119887
119894119887minus 119862119887
119899(120596119899
119894119890+ 120596119899
119890119899) (4)
and R119888is the velocity transformation matrix between the 119899
frame and the 119890 frame defined as
R119888=
[
[
[
[
sec120593119877 + ℎ
0 0
0
1
119877 + ℎ
0
0 0 1
]
]
]
]
(5)
The velocity v119899 can be measured by the DVL if 119862119899119887is
known The purpose of the alignment is to determine theattitude DCM 119862
119899
119887and the position p based on the data
collected from the accelerometers the gyroscopes and theDVL while in motion
3 Attitude Alignment
31 Attitude Coarse Alignment Since the velocity of the DVLis in the 119887 frame the aim here is to convert (2) into the 119887 frametoo Equation (2) can be written as
k119899 =119889 (119862119899
119887k119887)
119889119905
= 119899
119887k119887 + 119862
119899
119887k119887
= 119862119899
119887f119887 minus (2120596
119899
119894119890+ 120596119899
119890119899) times k119899 + g119899
(6)
Substituting (1) into (6) yields
k119887 = f119887 minus 119862119887
119899(2120596119899
119894119890+ 120596119899
119890119899) times (119862
119899
119887k119887) minus 120596119887
119899119887times k119887 + 119862
119887
119899g119899 (7)
This is the complete form of the velocity update equation inthe 119887 frame In fact 119862119887
119899(2120596119899
119894119890+120596119899
119890119899) times (119862
119899
119887k119887) could be omitted
for the reason that the AUVs would not sail in a very highspeed (commonly several meters per second) and it is thecoarse alignment stage The simplified form of (7) for thecoarse alignment is
k119887 = f119887 minus 120596119887119899119887times k119887 + 119862
119887
119899g119899 (8)
Equation (1) is the traditional attitude update equationNowadays the attitude update is separated using the DCMproduct chain rule as [12]
119862119899(119905+Δ119905)
119887(119905+Δ119905)= 119862119899(119905+Δ119905)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(119905+Δ119905) (9)
The update method for 119862119899(119905+Δ119905)119899(119905)
and 119862119887(119905)
119887(119905+Δ119905)is also proposed
in [12] If this rule is applied in each update cycle the attitudeupdate equation will be
119862119899(119905)
119887(119905)= 119862119899(119905)
119899(0)119862119899(0)
119887(0)119862119887(0)
119887(119905) (10)
The update equations for 119862119899(0)119899(119905)
and 119862119887(0)
119887(119905)are [11]
119899(0)
119899(119905)= 119862119899(0)
119899(119905)120596119899
119894119899times (11)
119887(0)
119887(119905)= 119862119887(0)
119887(119905)120596119887
119894119887times (12)
Mathematical Problems in Engineering 3
where 120596119899119894119899
= 120596119899
119894119890+ 120596119899
119890119899asymp 120596119899
119894119890for the reason that 120596119899
119890119899is
unknown but minor in alignment stage If 119862119899(0)119887(0)
is identified119862119899(119905)
119887(119905)on every time step is known The procedure identifying
119862119899(0)
119887(0)is as follows
Substitute (10) into (8) and multiply 119862119899(0)119899(119905)
on each part
119862119887(0)
119887(119905)k119887 minus 119862
119887(0)
119887(119905)f119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887 = 119862
119887(0)
119899(0)119862119899(0)
119899(119905)g119899 (13)
Integrating (13) on both sides
120572 (119905) = 119862119887(0)
119899(0)120573 (119905) (14)
where
120572 (119905) = int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887 minus 119862
119887(0)
119887(119905)f119887119889119905 (15)
120573 (119905) = int
119905
0
119862119899(0)
119899(119905)g119899119889119905 (16)
The initial attitude matrix119862119899(0)119887(0)
can be uniquely solved by theoptimization-basedmethod described in [16 17] to minimize
119871 (119862119887(0)
119899(0)) = sum(120572 (119905
119873) minus 119862119887(0)
119899(0)120573 (119905119873))
2
(17)
So the key is how to calculate 120572(119905) and 120573(119905) with differentintegral time
32 Calculation Scheme for 120572(t)120573(t) Calculation scheme for120572(119905) 120573(119905) includes two parts integration strategy and updatescheme
321 Integration Strategy If k119887 120596119887119894119887 and f119887 were known (13)
could be the model for calculating 119862119899(0)
119887(0) This cannot be
achieved by real sensors The DVLrsquos output is the velocity inthe 119887 frame the gyroscopersquos output is the incremental angleover the sample time and the accelerometerrsquos output is theincremental velocity over the sample time This is the reasonthat the integral forms are needed
For (15) notice that
int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905
= 119862119887(0)
119887(119905)k11988710038161003816100381610038161003816
119905
0minus int
119905
0
119887(0)
119887(119905)k119887 minus 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905
(18)
Substitute (4) and (12) into (18) and omit the 119862119887(0)119887(119905)
119862119887(119905)
119899(119905)(120596119899
119894119890+
120596119899
119890119899) times k119887 part for the same reason as the AUVs would not
voyage in a very high speed
int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905 asymp 119862
119887(0)
119887(119905)k11988710038161003816100381610038161003816
119905
0 (19)
The calculationmethods for the other part of (15) and thewhole equation (16) are already expressed in [11] as
int
119905
0
119862119887(0)
119887(119905)f119887119889119905 =
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)int
119905119896+1
119905119896
119862119887(119905119896)
119887(119905)f119887119889119905
asymp
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)int
119905119896+1
119905119896
[I + (int
119905
119905119896
120596119887
119894119887times 119889120591)] f119887119889119905
=
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[Δk1+ Δk2+
1
2
(Δ1205791+ Δ1205792)
times (Δk1+ Δk2)
+
2
3
(Δ1205791times Δk2+ Δk1times Δ1205792)]
(20)
120573 (119905119873) = int
119905
0
119862119899(0)
119899(119905)g119899119889119905 =
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)g119899119889119905
asymp
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
(I + (119905 minus 119905119896) 120596119899
119894119899times) g119899119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(119879I + 119879
2
2
120596119899
119894119899times) g119899
asymp
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(119879I + 119879
2
2
120596119899
119894119890times) g119899
(21)
So
120572 (119905119873) asymp 119862119887(0)
119887(119905119873)k119887 (119905119873) minus k119887 (0)
minus
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[Δk1+ Δk2+
1
2
(Δ1205791+ Δ1205792)
times (Δk1+ Δk2)
+
2
3
(Δ1205791times Δk2+ Δk1times Δ1205792)]
(22)
where the angular rate and the specific force are assumed inlinear forms within a short time as
120596119887
119894119887= (119905 minus 119905
119896) c120596+ d120596
f119887 = (119905 minus 119905119896) c119891+ d119891
(23)
The c119891 d119891 c120596 and d
120596are the coefficient vectors as
c119891=
4 (Δk2minus Δk1)
1198792
d119891=
3Δk1minus Δk2
119879
(24)
4 Mathematical Problems in Engineering
c120596=
4 (Δ1205792minus Δ1205791)
1198792
d120596=
3Δ1205791minus Δ1205792
119879
(25)
where 119879 is the time period of one update interval [119905119896
119905119896+1
] 119896 = 0 1 2 119873 minus 1 with 119905
119896= 119896119879 and Δv
1 Δv2and Δ120579
1
Δ1205792are the incremental velocity and angle collected from the
accelerometers and gyroscopes
322 Update Scheme Theupdate rate of the sensors is not thesame The update rate of the gyroscopes and accelerometerscan go up from 1 to 2 kHz while the update rate of the DVL isonly several Hz and the update may be interrupted due to thedepth of the water or some other reasonsThe update schemefor 120572(119905) 120573(119905) should be suitable for these conditions
For 120572(119905) expressed in (22) the lower part is related to thegyroscopes and accelerometers In order to obtain an accurateresult this part should be updated at these sensorsrsquo updaterate A higher update rate could reduce the effect such asconing motion [18] For the whole equation the update rateis based on the DVL It means that the lower part should beupdated in high speed and when the DVLrsquos data is availablethe whole equation is updated
For 120573(119905) expressed in (21) the update rate is the same asthe gyroscopes and accelerometers And the output rate iskept the same as 120572(119905) since the algorithm needs the data atthe same time point Consider
120572 (119905) 997888rarr 120572DVL (119905119872)
120573 (119905) 997888rarr 120573DVL (119905119872) (26)
where119872 = 0 1 2 119905119872means the time point that the DVL
is available The least number of 119872 is decided by the sensorerror level
Owing to the low and unsteady output rate of theDVL the v119887(0) is almost not obtainable This part can becounteracted as
120572DVL (119905119872) minus 120572DVL (1199050) = 119862119887(0)
119899(0)(120573DVL (119905119872) minus 120573DVL (1199050))
(27)
This is the practical equation for the coarse alignment
33 Attitude Fine Alignment According to the sensor errorlevel about 200 samples (119872 asymp 200) of DVL data are neededfor the coarse alignment Since the update rate of the DVL isseveral Hz the coarse alignment could cost about 60 s After60 s of the attitude coarse alignment the pitch and roll wouldattain about 01 deg accuracy and the heading error would bearound 10 deg The fine alignment starts immediately after itThe goal is to improve the attitude accuracy especially theheading accuracy
For the fine alignment most methods use the SINSattitude and velocity error models as the state transitionequation for the data fusion algorithms such as the Kalman
filter (KF) the extended KF (EKF) and the UKF The UKFfor its worthy performance in nonlinear estimation [19 20]is especially used for nonlinear misalignment angles Hereis the review of the UKF based attitude fine alignment withnonlinear misalignment angles model [7 21 22]
The navigation algorithm is described in (1) (2) and (3)The state of the UKF is
x = [120575k119899 120579 120575120596119887
119894119887120575f119887]119879
(28)
And the large misalignment angles model is
120575k119899 = (I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899
minus (2119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
120579 = (I minus 119862
1198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
120575119887
119894119887= 0 + 120576
120596 120575
f119887 = 0 + 120576119891
(29)
where 119910 is the real data equal to the ideal data 119910 plus the error120575119910 119862119899
1015840
119887is the DCM from the 119887 frame to the 1198991015840 frame 120576
120596and
120576119891are the noise of the gyroscopes and accelerometers and
1198621198991015840
119899is the DCM from the 119899 frame to the 119899
1015840 frame Assumethat the 119899 frame to the 1198991015840 frame is completed through threenoninterchangeable rotations 120579
119911 120579119909 and 120579
119910 Each rotation
can be expressed as
119862120579119909
=[
[
[
1 0 0
0 cos 120579119909
sin 120579119909
0 minus sin 120579119909
cos 120579119909
]
]
]
119862120579119910
=
[
[
[
[
cos 120579119910
0 minus sin 120579119910
0 1 0
sin 120579119910
0 cos 120579119910
]
]
]
]
119862120579119911
=[
[
[
cos 120579119911
sin 120579119911
0
minus sin 120579119911
cos 120579119911
0
0 0 1
]
]
]
(30)
The total DCM is
1198621198991015840
119899= 119862120579119910119862120579119909119862120579119911 (31)
The measurement model is
z = k119899 minus 119862119899
11989910158401198621198991015840
119887k119887DVL = k119899 + 120575k119899
minus 119862119899
11989910158401198621198991015840
119887(k119887DVL + 120575k119887DVL)
= k119899 minus k119899DVL + 120575k119899 minus 120575k119899DVL = 120575k119899 minus 120575k119899DVL
(32)
where 120575k119899DVL is the noise of the DVLThen according to (29) and (32) the nonlinear state and
measurement equations are
Mathematical Problems in Engineering 5
x =
[
[
[
[
[
[
[
[
[
120575k119899
120579
120575119887
119894119887
120575f119887
]
]
]
]
]
]
]
]
]
= 119891 (x) + w
=
[
[
[
[
[
[
[
[
(I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899 minus (2
119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
(I minus 1198621198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
0
0
]
]
]
]
]
]
]
]
+
[
[
[
[
0
0
120576120596
120576119891
]
]
]
]
z = ℎ (x) + u = [120575k119899] + [minus120575k119899DVL]
(33)
where w = [03times1 03times1120576120596120576119891] is the state noise and u =
[minus120575k119899DVL] is the measurement noiseThe fine alignmentrsquos duty is to estimate 119862
119899
1198991015840 to improve
the accuracy of 119862119899119887 Notice that 119862119899(0)
119887(0) 119862119887(0)119887(119905)
and 119862119899(0)
119899(119905)can still
be updated in fine alignment It means that a more precise119862119899(0)
119887(0)is realized too according to the inverse of (10) in the fine
alignment stage as
119862119899(0)
119887(0)= 119862119899(0)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(0) (34)
This result is of great importance in the position alignment
4 Position Alignment
41 Position Alignment Algorithm The original positionupdate equation is described in (3) This position updateequation can also be performed between the 119890 frame and the119887 frame as
119862119899(0)
119899(119905)Rminus1119888p (119905) = 119862
119899(0)
119887(0)119862119887(0)
119887(119905)k119887 (119905) (35)
In order to get the position (35) should be integrated on eachside first The integral form of the left term is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
(36)
where 119905end is the time point where the attitude alignmentends Substitute (11) into (36)
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(37)
The integral part of (37) can be converted as
int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(38)
where 119905119896= 119905end when 119896 = 119873 Practically the effort of the
position change is really small in the integration It could beregarded as constant as
p (119905) = p (119905end) (39)
where p(119905end) is the position which needs alignment Substi-tute (39) into the integral part of (38)
int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
asymp int
119905119896+1
119905119896
(I + (119905 minus 119905119896)120596119899
119894119899times)120596119899
119894119899times Rminus1119888p (119905end) 119889119905
= 120596119899
119894119899times Rminus1119888p (119905end) 119879 + 120596
119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(40)
Therefore the computed form of (36) is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(41)
For (41) 120596119899119894119899times120596119899
119894119899timesRminus1119888p(119905end) 11987922 which is a second-order
term is much smaller than 120596119899119894119899times Rminus1119888p(119905end)119879 with 119879 lt 1 So
6 Mathematical Problems in Engineering
this part could be omittedThen notice that119879120596119899119894119899times asymp C119899(119905119896)
119899(119905119896+1)minus
I (41) can be simplified as
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(C119899(119905119896)119899(119905119896+1)
minus I)Rminus1119888p (119905end)
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus [119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (119905end)]
= Rminus1119888p (119905end) minus Rminus1
119888p (0)
(42)
The integral form of the right term of (35) is
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
= 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
119862
119887(119905119895)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
(I + (int
119905
119905119895
120596119887
119894119887119889120591)times) k119887119889119905
(43)
where 119905119895= 119905end when 119895 = 119872 Similarly v119887 can be approxi-
mated linearly as
k119887 (119905) = k119887 (119905119895) +
119905 minus 119905119895
119879DVL(k119887 (119905
119895+1) minus k119887 (119905
119895)) (44)
where 119879DVL is the duration of two DVL samples Substitute(23) (25) and (44) into (43)
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(45)
According to (35) (42) equals (45) It follows that
p (119905end) = p (0)
+ R119888119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896))
+
1
3
Δ1205791times 119879DVLk
119887(119905119896)
+ (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(46)
For (46) all the data can be calculated in real-time except119862119899(0)
119887(0) It means that when the initial attitude matrix 119862
119899(0)
119887(0)
is obtained the position could immediately be updated Asdescribed in Section 3 119862119899(0)
119887(0)can be gathered by (27) through
the attitude coarse alignment and then the accuracy can beimproved by (34) through the whole attitude fine alignment
Also (39) can be treated as
p (119905) = p (0) (47)
And then (41) goes to
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879 + 120596
119899
119894119899
times120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(48)
And by making (48) be equal to (45) another solution comesout as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(49)
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
2 Mathematical Problems in Engineering
the GPS is not available for underwater assignments Themain idea is to use the DVL to reckon the coordinate fromthe start point The problem is that the body frame velocitycannot be integrated into position before the attitude isknown
The contents are organized as follows Section 2 explainsthe problem in mathematics Section 3 is the attitude align-ment section The attitude coarse alignment is devised byfurther study of the velocity update equation in the nav-igation frame and its transformation in the body frameOptimization approach is applied between the body framevelocity and the integral formof the gravity Also the commonlarge misalignment angles model based UKF is reviewedin Section 3 Section 4 proposed the position alignment byseparately treating the body frame velocity and the Earthframe position And when the initial attitude is gathered theposition could be straight obtained through an equationTheperformance of the algorithm is evaluated with simulationin Section 5 and on-lake tests in Section 6 Conclusions aredrawn in Section 7
2 Problem Statement
The coordinate frames used for the in-motion alignment aredefined as follows
(i) The 119899 frame is the ideal local level navigation coordi-nate frame with east-north-up geodetic axes
(ii) The 1198991015840 frame is the real local level navigation coordi-nate frame there is some error between 119899 and 1198991015840 frameowing to the alignment error and the sensor error
(iii) The 119887 frame is the strapdown inertial sensorrsquos bodycoordinate frame
(iv) The 119890 frame is the Earth coordinate frame(v) The 119894 frame is the nonrotating inertial coordinate
frame
The common navigation (attitude velocity and position)update equations are written in the 119899 frame as [12ndash15]
119899
119887= 119862119899
119887120596119887
119899119887times (1)
k119899 = 119862119899
119887f119887 minus (2120596
119899
119894119890+ 120596119899
119890119899) times k119899 + g119899 (2)
p = R119888k119899 (3)
where 119862119899119887is the attitude direction cosine matrix (DCM) from
the 119887 frame to the 119899 frame v119899 is the velocity in the 119899 framef119887 is the special force measured by the accelerometers fixedin the 119887 frame 120596119899
119894119890is the angular rate of the Earthrsquos rotation
in the 119899 frame 120596119899119890119899is the angular rate caused by the velocity
on the Earth (since the Earth is nearly round) in the 119899 frameg119899 is the gravity in the 119899 frame (sdot)times is the matrix form of across-product which satisfies a times b = (atimes)b p is the positiondescribed in the 119890 frame as p = [120582 120593 ℎ]
119879 120582 is the longitude120593 is the latitude ℎ is the height above the mean sea level 119877is the radius of the Earth 120596119887
119894119887is the angular rate measured by
the gyroscopes in the 119887 frame 120596119887119899119887
is the angular rate of theSINS in the 119887 frame with respect to the 119899 frame described as
120596119887
119899119887= 120596119887
119894119887minus 119862119887
119899(120596119899
119894119890+ 120596119899
119890119899) (4)
and R119888is the velocity transformation matrix between the 119899
frame and the 119890 frame defined as
R119888=
[
[
[
[
sec120593119877 + ℎ
0 0
0
1
119877 + ℎ
0
0 0 1
]
]
]
]
(5)
The velocity v119899 can be measured by the DVL if 119862119899119887is
known The purpose of the alignment is to determine theattitude DCM 119862
119899
119887and the position p based on the data
collected from the accelerometers the gyroscopes and theDVL while in motion
3 Attitude Alignment
31 Attitude Coarse Alignment Since the velocity of the DVLis in the 119887 frame the aim here is to convert (2) into the 119887 frametoo Equation (2) can be written as
k119899 =119889 (119862119899
119887k119887)
119889119905
= 119899
119887k119887 + 119862
119899
119887k119887
= 119862119899
119887f119887 minus (2120596
119899
119894119890+ 120596119899
119890119899) times k119899 + g119899
(6)
Substituting (1) into (6) yields
k119887 = f119887 minus 119862119887
119899(2120596119899
119894119890+ 120596119899
119890119899) times (119862
119899
119887k119887) minus 120596119887
119899119887times k119887 + 119862
119887
119899g119899 (7)
This is the complete form of the velocity update equation inthe 119887 frame In fact 119862119887
119899(2120596119899
119894119890+120596119899
119890119899) times (119862
119899
119887k119887) could be omitted
for the reason that the AUVs would not sail in a very highspeed (commonly several meters per second) and it is thecoarse alignment stage The simplified form of (7) for thecoarse alignment is
k119887 = f119887 minus 120596119887119899119887times k119887 + 119862
119887
119899g119899 (8)
Equation (1) is the traditional attitude update equationNowadays the attitude update is separated using the DCMproduct chain rule as [12]
119862119899(119905+Δ119905)
119887(119905+Δ119905)= 119862119899(119905+Δ119905)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(119905+Δ119905) (9)
The update method for 119862119899(119905+Δ119905)119899(119905)
and 119862119887(119905)
119887(119905+Δ119905)is also proposed
in [12] If this rule is applied in each update cycle the attitudeupdate equation will be
119862119899(119905)
119887(119905)= 119862119899(119905)
119899(0)119862119899(0)
119887(0)119862119887(0)
119887(119905) (10)
The update equations for 119862119899(0)119899(119905)
and 119862119887(0)
119887(119905)are [11]
119899(0)
119899(119905)= 119862119899(0)
119899(119905)120596119899
119894119899times (11)
119887(0)
119887(119905)= 119862119887(0)
119887(119905)120596119887
119894119887times (12)
Mathematical Problems in Engineering 3
where 120596119899119894119899
= 120596119899
119894119890+ 120596119899
119890119899asymp 120596119899
119894119890for the reason that 120596119899
119890119899is
unknown but minor in alignment stage If 119862119899(0)119887(0)
is identified119862119899(119905)
119887(119905)on every time step is known The procedure identifying
119862119899(0)
119887(0)is as follows
Substitute (10) into (8) and multiply 119862119899(0)119899(119905)
on each part
119862119887(0)
119887(119905)k119887 minus 119862
119887(0)
119887(119905)f119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887 = 119862
119887(0)
119899(0)119862119899(0)
119899(119905)g119899 (13)
Integrating (13) on both sides
120572 (119905) = 119862119887(0)
119899(0)120573 (119905) (14)
where
120572 (119905) = int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887 minus 119862
119887(0)
119887(119905)f119887119889119905 (15)
120573 (119905) = int
119905
0
119862119899(0)
119899(119905)g119899119889119905 (16)
The initial attitude matrix119862119899(0)119887(0)
can be uniquely solved by theoptimization-basedmethod described in [16 17] to minimize
119871 (119862119887(0)
119899(0)) = sum(120572 (119905
119873) minus 119862119887(0)
119899(0)120573 (119905119873))
2
(17)
So the key is how to calculate 120572(119905) and 120573(119905) with differentintegral time
32 Calculation Scheme for 120572(t)120573(t) Calculation scheme for120572(119905) 120573(119905) includes two parts integration strategy and updatescheme
321 Integration Strategy If k119887 120596119887119894119887 and f119887 were known (13)
could be the model for calculating 119862119899(0)
119887(0) This cannot be
achieved by real sensors The DVLrsquos output is the velocity inthe 119887 frame the gyroscopersquos output is the incremental angleover the sample time and the accelerometerrsquos output is theincremental velocity over the sample time This is the reasonthat the integral forms are needed
For (15) notice that
int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905
= 119862119887(0)
119887(119905)k11988710038161003816100381610038161003816
119905
0minus int
119905
0
119887(0)
119887(119905)k119887 minus 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905
(18)
Substitute (4) and (12) into (18) and omit the 119862119887(0)119887(119905)
119862119887(119905)
119899(119905)(120596119899
119894119890+
120596119899
119890119899) times k119887 part for the same reason as the AUVs would not
voyage in a very high speed
int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905 asymp 119862
119887(0)
119887(119905)k11988710038161003816100381610038161003816
119905
0 (19)
The calculationmethods for the other part of (15) and thewhole equation (16) are already expressed in [11] as
int
119905
0
119862119887(0)
119887(119905)f119887119889119905 =
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)int
119905119896+1
119905119896
119862119887(119905119896)
119887(119905)f119887119889119905
asymp
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)int
119905119896+1
119905119896
[I + (int
119905
119905119896
120596119887
119894119887times 119889120591)] f119887119889119905
=
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[Δk1+ Δk2+
1
2
(Δ1205791+ Δ1205792)
times (Δk1+ Δk2)
+
2
3
(Δ1205791times Δk2+ Δk1times Δ1205792)]
(20)
120573 (119905119873) = int
119905
0
119862119899(0)
119899(119905)g119899119889119905 =
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)g119899119889119905
asymp
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
(I + (119905 minus 119905119896) 120596119899
119894119899times) g119899119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(119879I + 119879
2
2
120596119899
119894119899times) g119899
asymp
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(119879I + 119879
2
2
120596119899
119894119890times) g119899
(21)
So
120572 (119905119873) asymp 119862119887(0)
119887(119905119873)k119887 (119905119873) minus k119887 (0)
minus
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[Δk1+ Δk2+
1
2
(Δ1205791+ Δ1205792)
times (Δk1+ Δk2)
+
2
3
(Δ1205791times Δk2+ Δk1times Δ1205792)]
(22)
where the angular rate and the specific force are assumed inlinear forms within a short time as
120596119887
119894119887= (119905 minus 119905
119896) c120596+ d120596
f119887 = (119905 minus 119905119896) c119891+ d119891
(23)
The c119891 d119891 c120596 and d
120596are the coefficient vectors as
c119891=
4 (Δk2minus Δk1)
1198792
d119891=
3Δk1minus Δk2
119879
(24)
4 Mathematical Problems in Engineering
c120596=
4 (Δ1205792minus Δ1205791)
1198792
d120596=
3Δ1205791minus Δ1205792
119879
(25)
where 119879 is the time period of one update interval [119905119896
119905119896+1
] 119896 = 0 1 2 119873 minus 1 with 119905
119896= 119896119879 and Δv
1 Δv2and Δ120579
1
Δ1205792are the incremental velocity and angle collected from the
accelerometers and gyroscopes
322 Update Scheme Theupdate rate of the sensors is not thesame The update rate of the gyroscopes and accelerometerscan go up from 1 to 2 kHz while the update rate of the DVL isonly several Hz and the update may be interrupted due to thedepth of the water or some other reasonsThe update schemefor 120572(119905) 120573(119905) should be suitable for these conditions
For 120572(119905) expressed in (22) the lower part is related to thegyroscopes and accelerometers In order to obtain an accurateresult this part should be updated at these sensorsrsquo updaterate A higher update rate could reduce the effect such asconing motion [18] For the whole equation the update rateis based on the DVL It means that the lower part should beupdated in high speed and when the DVLrsquos data is availablethe whole equation is updated
For 120573(119905) expressed in (21) the update rate is the same asthe gyroscopes and accelerometers And the output rate iskept the same as 120572(119905) since the algorithm needs the data atthe same time point Consider
120572 (119905) 997888rarr 120572DVL (119905119872)
120573 (119905) 997888rarr 120573DVL (119905119872) (26)
where119872 = 0 1 2 119905119872means the time point that the DVL
is available The least number of 119872 is decided by the sensorerror level
Owing to the low and unsteady output rate of theDVL the v119887(0) is almost not obtainable This part can becounteracted as
120572DVL (119905119872) minus 120572DVL (1199050) = 119862119887(0)
119899(0)(120573DVL (119905119872) minus 120573DVL (1199050))
(27)
This is the practical equation for the coarse alignment
33 Attitude Fine Alignment According to the sensor errorlevel about 200 samples (119872 asymp 200) of DVL data are neededfor the coarse alignment Since the update rate of the DVL isseveral Hz the coarse alignment could cost about 60 s After60 s of the attitude coarse alignment the pitch and roll wouldattain about 01 deg accuracy and the heading error would bearound 10 deg The fine alignment starts immediately after itThe goal is to improve the attitude accuracy especially theheading accuracy
For the fine alignment most methods use the SINSattitude and velocity error models as the state transitionequation for the data fusion algorithms such as the Kalman
filter (KF) the extended KF (EKF) and the UKF The UKFfor its worthy performance in nonlinear estimation [19 20]is especially used for nonlinear misalignment angles Hereis the review of the UKF based attitude fine alignment withnonlinear misalignment angles model [7 21 22]
The navigation algorithm is described in (1) (2) and (3)The state of the UKF is
x = [120575k119899 120579 120575120596119887
119894119887120575f119887]119879
(28)
And the large misalignment angles model is
120575k119899 = (I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899
minus (2119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
120579 = (I minus 119862
1198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
120575119887
119894119887= 0 + 120576
120596 120575
f119887 = 0 + 120576119891
(29)
where 119910 is the real data equal to the ideal data 119910 plus the error120575119910 119862119899
1015840
119887is the DCM from the 119887 frame to the 1198991015840 frame 120576
120596and
120576119891are the noise of the gyroscopes and accelerometers and
1198621198991015840
119899is the DCM from the 119899 frame to the 119899
1015840 frame Assumethat the 119899 frame to the 1198991015840 frame is completed through threenoninterchangeable rotations 120579
119911 120579119909 and 120579
119910 Each rotation
can be expressed as
119862120579119909
=[
[
[
1 0 0
0 cos 120579119909
sin 120579119909
0 minus sin 120579119909
cos 120579119909
]
]
]
119862120579119910
=
[
[
[
[
cos 120579119910
0 minus sin 120579119910
0 1 0
sin 120579119910
0 cos 120579119910
]
]
]
]
119862120579119911
=[
[
[
cos 120579119911
sin 120579119911
0
minus sin 120579119911
cos 120579119911
0
0 0 1
]
]
]
(30)
The total DCM is
1198621198991015840
119899= 119862120579119910119862120579119909119862120579119911 (31)
The measurement model is
z = k119899 minus 119862119899
11989910158401198621198991015840
119887k119887DVL = k119899 + 120575k119899
minus 119862119899
11989910158401198621198991015840
119887(k119887DVL + 120575k119887DVL)
= k119899 minus k119899DVL + 120575k119899 minus 120575k119899DVL = 120575k119899 minus 120575k119899DVL
(32)
where 120575k119899DVL is the noise of the DVLThen according to (29) and (32) the nonlinear state and
measurement equations are
Mathematical Problems in Engineering 5
x =
[
[
[
[
[
[
[
[
[
120575k119899
120579
120575119887
119894119887
120575f119887
]
]
]
]
]
]
]
]
]
= 119891 (x) + w
=
[
[
[
[
[
[
[
[
(I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899 minus (2
119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
(I minus 1198621198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
0
0
]
]
]
]
]
]
]
]
+
[
[
[
[
0
0
120576120596
120576119891
]
]
]
]
z = ℎ (x) + u = [120575k119899] + [minus120575k119899DVL]
(33)
where w = [03times1 03times1120576120596120576119891] is the state noise and u =
[minus120575k119899DVL] is the measurement noiseThe fine alignmentrsquos duty is to estimate 119862
119899
1198991015840 to improve
the accuracy of 119862119899119887 Notice that 119862119899(0)
119887(0) 119862119887(0)119887(119905)
and 119862119899(0)
119899(119905)can still
be updated in fine alignment It means that a more precise119862119899(0)
119887(0)is realized too according to the inverse of (10) in the fine
alignment stage as
119862119899(0)
119887(0)= 119862119899(0)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(0) (34)
This result is of great importance in the position alignment
4 Position Alignment
41 Position Alignment Algorithm The original positionupdate equation is described in (3) This position updateequation can also be performed between the 119890 frame and the119887 frame as
119862119899(0)
119899(119905)Rminus1119888p (119905) = 119862
119899(0)
119887(0)119862119887(0)
119887(119905)k119887 (119905) (35)
In order to get the position (35) should be integrated on eachside first The integral form of the left term is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
(36)
where 119905end is the time point where the attitude alignmentends Substitute (11) into (36)
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(37)
The integral part of (37) can be converted as
int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(38)
where 119905119896= 119905end when 119896 = 119873 Practically the effort of the
position change is really small in the integration It could beregarded as constant as
p (119905) = p (119905end) (39)
where p(119905end) is the position which needs alignment Substi-tute (39) into the integral part of (38)
int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
asymp int
119905119896+1
119905119896
(I + (119905 minus 119905119896)120596119899
119894119899times)120596119899
119894119899times Rminus1119888p (119905end) 119889119905
= 120596119899
119894119899times Rminus1119888p (119905end) 119879 + 120596
119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(40)
Therefore the computed form of (36) is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(41)
For (41) 120596119899119894119899times120596119899
119894119899timesRminus1119888p(119905end) 11987922 which is a second-order
term is much smaller than 120596119899119894119899times Rminus1119888p(119905end)119879 with 119879 lt 1 So
6 Mathematical Problems in Engineering
this part could be omittedThen notice that119879120596119899119894119899times asymp C119899(119905119896)
119899(119905119896+1)minus
I (41) can be simplified as
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(C119899(119905119896)119899(119905119896+1)
minus I)Rminus1119888p (119905end)
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus [119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (119905end)]
= Rminus1119888p (119905end) minus Rminus1
119888p (0)
(42)
The integral form of the right term of (35) is
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
= 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
119862
119887(119905119895)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
(I + (int
119905
119905119895
120596119887
119894119887119889120591)times) k119887119889119905
(43)
where 119905119895= 119905end when 119895 = 119872 Similarly v119887 can be approxi-
mated linearly as
k119887 (119905) = k119887 (119905119895) +
119905 minus 119905119895
119879DVL(k119887 (119905
119895+1) minus k119887 (119905
119895)) (44)
where 119879DVL is the duration of two DVL samples Substitute(23) (25) and (44) into (43)
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(45)
According to (35) (42) equals (45) It follows that
p (119905end) = p (0)
+ R119888119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896))
+
1
3
Δ1205791times 119879DVLk
119887(119905119896)
+ (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(46)
For (46) all the data can be calculated in real-time except119862119899(0)
119887(0) It means that when the initial attitude matrix 119862
119899(0)
119887(0)
is obtained the position could immediately be updated Asdescribed in Section 3 119862119899(0)
119887(0)can be gathered by (27) through
the attitude coarse alignment and then the accuracy can beimproved by (34) through the whole attitude fine alignment
Also (39) can be treated as
p (119905) = p (0) (47)
And then (41) goes to
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879 + 120596
119899
119894119899
times120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(48)
And by making (48) be equal to (45) another solution comesout as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(49)
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 3
where 120596119899119894119899
= 120596119899
119894119890+ 120596119899
119890119899asymp 120596119899
119894119890for the reason that 120596119899
119890119899is
unknown but minor in alignment stage If 119862119899(0)119887(0)
is identified119862119899(119905)
119887(119905)on every time step is known The procedure identifying
119862119899(0)
119887(0)is as follows
Substitute (10) into (8) and multiply 119862119899(0)119899(119905)
on each part
119862119887(0)
119887(119905)k119887 minus 119862
119887(0)
119887(119905)f119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887 = 119862
119887(0)
119899(0)119862119899(0)
119899(119905)g119899 (13)
Integrating (13) on both sides
120572 (119905) = 119862119887(0)
119899(0)120573 (119905) (14)
where
120572 (119905) = int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887 minus 119862
119887(0)
119887(119905)f119887119889119905 (15)
120573 (119905) = int
119905
0
119862119899(0)
119899(119905)g119899119889119905 (16)
The initial attitude matrix119862119899(0)119887(0)
can be uniquely solved by theoptimization-basedmethod described in [16 17] to minimize
119871 (119862119887(0)
119899(0)) = sum(120572 (119905
119873) minus 119862119887(0)
119899(0)120573 (119905119873))
2
(17)
So the key is how to calculate 120572(119905) and 120573(119905) with differentintegral time
32 Calculation Scheme for 120572(t)120573(t) Calculation scheme for120572(119905) 120573(119905) includes two parts integration strategy and updatescheme
321 Integration Strategy If k119887 120596119887119894119887 and f119887 were known (13)
could be the model for calculating 119862119899(0)
119887(0) This cannot be
achieved by real sensors The DVLrsquos output is the velocity inthe 119887 frame the gyroscopersquos output is the incremental angleover the sample time and the accelerometerrsquos output is theincremental velocity over the sample time This is the reasonthat the integral forms are needed
For (15) notice that
int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905
= 119862119887(0)
119887(119905)k11988710038161003816100381610038161003816
119905
0minus int
119905
0
119887(0)
119887(119905)k119887 minus 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905
(18)
Substitute (4) and (12) into (18) and omit the 119862119887(0)119887(119905)
119862119887(119905)
119899(119905)(120596119899
119894119890+
120596119899
119890119899) times k119887 part for the same reason as the AUVs would not
voyage in a very high speed
int
119905
0
119862119887(0)
119887(119905)k119887 + 119862
119887(0)
119887(119905)120596119887
119899119887times k119887119889119905 asymp 119862
119887(0)
119887(119905)k11988710038161003816100381610038161003816
119905
0 (19)
The calculationmethods for the other part of (15) and thewhole equation (16) are already expressed in [11] as
int
119905
0
119862119887(0)
119887(119905)f119887119889119905 =
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)int
119905119896+1
119905119896
119862119887(119905119896)
119887(119905)f119887119889119905
asymp
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)int
119905119896+1
119905119896
[I + (int
119905
119905119896
120596119887
119894119887times 119889120591)] f119887119889119905
=
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[Δk1+ Δk2+
1
2
(Δ1205791+ Δ1205792)
times (Δk1+ Δk2)
+
2
3
(Δ1205791times Δk2+ Δk1times Δ1205792)]
(20)
120573 (119905119873) = int
119905
0
119862119899(0)
119899(119905)g119899119889119905 =
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)g119899119889119905
asymp
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
(I + (119905 minus 119905119896) 120596119899
119894119899times) g119899119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(119879I + 119879
2
2
120596119899
119894119899times) g119899
asymp
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(119879I + 119879
2
2
120596119899
119894119890times) g119899
(21)
So
120572 (119905119873) asymp 119862119887(0)
119887(119905119873)k119887 (119905119873) minus k119887 (0)
minus
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[Δk1+ Δk2+
1
2
(Δ1205791+ Δ1205792)
times (Δk1+ Δk2)
+
2
3
(Δ1205791times Δk2+ Δk1times Δ1205792)]
(22)
where the angular rate and the specific force are assumed inlinear forms within a short time as
120596119887
119894119887= (119905 minus 119905
119896) c120596+ d120596
f119887 = (119905 minus 119905119896) c119891+ d119891
(23)
The c119891 d119891 c120596 and d
120596are the coefficient vectors as
c119891=
4 (Δk2minus Δk1)
1198792
d119891=
3Δk1minus Δk2
119879
(24)
4 Mathematical Problems in Engineering
c120596=
4 (Δ1205792minus Δ1205791)
1198792
d120596=
3Δ1205791minus Δ1205792
119879
(25)
where 119879 is the time period of one update interval [119905119896
119905119896+1
] 119896 = 0 1 2 119873 minus 1 with 119905
119896= 119896119879 and Δv
1 Δv2and Δ120579
1
Δ1205792are the incremental velocity and angle collected from the
accelerometers and gyroscopes
322 Update Scheme Theupdate rate of the sensors is not thesame The update rate of the gyroscopes and accelerometerscan go up from 1 to 2 kHz while the update rate of the DVL isonly several Hz and the update may be interrupted due to thedepth of the water or some other reasonsThe update schemefor 120572(119905) 120573(119905) should be suitable for these conditions
For 120572(119905) expressed in (22) the lower part is related to thegyroscopes and accelerometers In order to obtain an accurateresult this part should be updated at these sensorsrsquo updaterate A higher update rate could reduce the effect such asconing motion [18] For the whole equation the update rateis based on the DVL It means that the lower part should beupdated in high speed and when the DVLrsquos data is availablethe whole equation is updated
For 120573(119905) expressed in (21) the update rate is the same asthe gyroscopes and accelerometers And the output rate iskept the same as 120572(119905) since the algorithm needs the data atthe same time point Consider
120572 (119905) 997888rarr 120572DVL (119905119872)
120573 (119905) 997888rarr 120573DVL (119905119872) (26)
where119872 = 0 1 2 119905119872means the time point that the DVL
is available The least number of 119872 is decided by the sensorerror level
Owing to the low and unsteady output rate of theDVL the v119887(0) is almost not obtainable This part can becounteracted as
120572DVL (119905119872) minus 120572DVL (1199050) = 119862119887(0)
119899(0)(120573DVL (119905119872) minus 120573DVL (1199050))
(27)
This is the practical equation for the coarse alignment
33 Attitude Fine Alignment According to the sensor errorlevel about 200 samples (119872 asymp 200) of DVL data are neededfor the coarse alignment Since the update rate of the DVL isseveral Hz the coarse alignment could cost about 60 s After60 s of the attitude coarse alignment the pitch and roll wouldattain about 01 deg accuracy and the heading error would bearound 10 deg The fine alignment starts immediately after itThe goal is to improve the attitude accuracy especially theheading accuracy
For the fine alignment most methods use the SINSattitude and velocity error models as the state transitionequation for the data fusion algorithms such as the Kalman
filter (KF) the extended KF (EKF) and the UKF The UKFfor its worthy performance in nonlinear estimation [19 20]is especially used for nonlinear misalignment angles Hereis the review of the UKF based attitude fine alignment withnonlinear misalignment angles model [7 21 22]
The navigation algorithm is described in (1) (2) and (3)The state of the UKF is
x = [120575k119899 120579 120575120596119887
119894119887120575f119887]119879
(28)
And the large misalignment angles model is
120575k119899 = (I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899
minus (2119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
120579 = (I minus 119862
1198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
120575119887
119894119887= 0 + 120576
120596 120575
f119887 = 0 + 120576119891
(29)
where 119910 is the real data equal to the ideal data 119910 plus the error120575119910 119862119899
1015840
119887is the DCM from the 119887 frame to the 1198991015840 frame 120576
120596and
120576119891are the noise of the gyroscopes and accelerometers and
1198621198991015840
119899is the DCM from the 119899 frame to the 119899
1015840 frame Assumethat the 119899 frame to the 1198991015840 frame is completed through threenoninterchangeable rotations 120579
119911 120579119909 and 120579
119910 Each rotation
can be expressed as
119862120579119909
=[
[
[
1 0 0
0 cos 120579119909
sin 120579119909
0 minus sin 120579119909
cos 120579119909
]
]
]
119862120579119910
=
[
[
[
[
cos 120579119910
0 minus sin 120579119910
0 1 0
sin 120579119910
0 cos 120579119910
]
]
]
]
119862120579119911
=[
[
[
cos 120579119911
sin 120579119911
0
minus sin 120579119911
cos 120579119911
0
0 0 1
]
]
]
(30)
The total DCM is
1198621198991015840
119899= 119862120579119910119862120579119909119862120579119911 (31)
The measurement model is
z = k119899 minus 119862119899
11989910158401198621198991015840
119887k119887DVL = k119899 + 120575k119899
minus 119862119899
11989910158401198621198991015840
119887(k119887DVL + 120575k119887DVL)
= k119899 minus k119899DVL + 120575k119899 minus 120575k119899DVL = 120575k119899 minus 120575k119899DVL
(32)
where 120575k119899DVL is the noise of the DVLThen according to (29) and (32) the nonlinear state and
measurement equations are
Mathematical Problems in Engineering 5
x =
[
[
[
[
[
[
[
[
[
120575k119899
120579
120575119887
119894119887
120575f119887
]
]
]
]
]
]
]
]
]
= 119891 (x) + w
=
[
[
[
[
[
[
[
[
(I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899 minus (2
119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
(I minus 1198621198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
0
0
]
]
]
]
]
]
]
]
+
[
[
[
[
0
0
120576120596
120576119891
]
]
]
]
z = ℎ (x) + u = [120575k119899] + [minus120575k119899DVL]
(33)
where w = [03times1 03times1120576120596120576119891] is the state noise and u =
[minus120575k119899DVL] is the measurement noiseThe fine alignmentrsquos duty is to estimate 119862
119899
1198991015840 to improve
the accuracy of 119862119899119887 Notice that 119862119899(0)
119887(0) 119862119887(0)119887(119905)
and 119862119899(0)
119899(119905)can still
be updated in fine alignment It means that a more precise119862119899(0)
119887(0)is realized too according to the inverse of (10) in the fine
alignment stage as
119862119899(0)
119887(0)= 119862119899(0)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(0) (34)
This result is of great importance in the position alignment
4 Position Alignment
41 Position Alignment Algorithm The original positionupdate equation is described in (3) This position updateequation can also be performed between the 119890 frame and the119887 frame as
119862119899(0)
119899(119905)Rminus1119888p (119905) = 119862
119899(0)
119887(0)119862119887(0)
119887(119905)k119887 (119905) (35)
In order to get the position (35) should be integrated on eachside first The integral form of the left term is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
(36)
where 119905end is the time point where the attitude alignmentends Substitute (11) into (36)
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(37)
The integral part of (37) can be converted as
int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(38)
where 119905119896= 119905end when 119896 = 119873 Practically the effort of the
position change is really small in the integration It could beregarded as constant as
p (119905) = p (119905end) (39)
where p(119905end) is the position which needs alignment Substi-tute (39) into the integral part of (38)
int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
asymp int
119905119896+1
119905119896
(I + (119905 minus 119905119896)120596119899
119894119899times)120596119899
119894119899times Rminus1119888p (119905end) 119889119905
= 120596119899
119894119899times Rminus1119888p (119905end) 119879 + 120596
119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(40)
Therefore the computed form of (36) is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(41)
For (41) 120596119899119894119899times120596119899
119894119899timesRminus1119888p(119905end) 11987922 which is a second-order
term is much smaller than 120596119899119894119899times Rminus1119888p(119905end)119879 with 119879 lt 1 So
6 Mathematical Problems in Engineering
this part could be omittedThen notice that119879120596119899119894119899times asymp C119899(119905119896)
119899(119905119896+1)minus
I (41) can be simplified as
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(C119899(119905119896)119899(119905119896+1)
minus I)Rminus1119888p (119905end)
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus [119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (119905end)]
= Rminus1119888p (119905end) minus Rminus1
119888p (0)
(42)
The integral form of the right term of (35) is
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
= 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
119862
119887(119905119895)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
(I + (int
119905
119905119895
120596119887
119894119887119889120591)times) k119887119889119905
(43)
where 119905119895= 119905end when 119895 = 119872 Similarly v119887 can be approxi-
mated linearly as
k119887 (119905) = k119887 (119905119895) +
119905 minus 119905119895
119879DVL(k119887 (119905
119895+1) minus k119887 (119905
119895)) (44)
where 119879DVL is the duration of two DVL samples Substitute(23) (25) and (44) into (43)
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(45)
According to (35) (42) equals (45) It follows that
p (119905end) = p (0)
+ R119888119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896))
+
1
3
Δ1205791times 119879DVLk
119887(119905119896)
+ (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(46)
For (46) all the data can be calculated in real-time except119862119899(0)
119887(0) It means that when the initial attitude matrix 119862
119899(0)
119887(0)
is obtained the position could immediately be updated Asdescribed in Section 3 119862119899(0)
119887(0)can be gathered by (27) through
the attitude coarse alignment and then the accuracy can beimproved by (34) through the whole attitude fine alignment
Also (39) can be treated as
p (119905) = p (0) (47)
And then (41) goes to
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879 + 120596
119899
119894119899
times120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(48)
And by making (48) be equal to (45) another solution comesout as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(49)
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
4 Mathematical Problems in Engineering
c120596=
4 (Δ1205792minus Δ1205791)
1198792
d120596=
3Δ1205791minus Δ1205792
119879
(25)
where 119879 is the time period of one update interval [119905119896
119905119896+1
] 119896 = 0 1 2 119873 minus 1 with 119905
119896= 119896119879 and Δv
1 Δv2and Δ120579
1
Δ1205792are the incremental velocity and angle collected from the
accelerometers and gyroscopes
322 Update Scheme Theupdate rate of the sensors is not thesame The update rate of the gyroscopes and accelerometerscan go up from 1 to 2 kHz while the update rate of the DVL isonly several Hz and the update may be interrupted due to thedepth of the water or some other reasonsThe update schemefor 120572(119905) 120573(119905) should be suitable for these conditions
For 120572(119905) expressed in (22) the lower part is related to thegyroscopes and accelerometers In order to obtain an accurateresult this part should be updated at these sensorsrsquo updaterate A higher update rate could reduce the effect such asconing motion [18] For the whole equation the update rateis based on the DVL It means that the lower part should beupdated in high speed and when the DVLrsquos data is availablethe whole equation is updated
For 120573(119905) expressed in (21) the update rate is the same asthe gyroscopes and accelerometers And the output rate iskept the same as 120572(119905) since the algorithm needs the data atthe same time point Consider
120572 (119905) 997888rarr 120572DVL (119905119872)
120573 (119905) 997888rarr 120573DVL (119905119872) (26)
where119872 = 0 1 2 119905119872means the time point that the DVL
is available The least number of 119872 is decided by the sensorerror level
Owing to the low and unsteady output rate of theDVL the v119887(0) is almost not obtainable This part can becounteracted as
120572DVL (119905119872) minus 120572DVL (1199050) = 119862119887(0)
119899(0)(120573DVL (119905119872) minus 120573DVL (1199050))
(27)
This is the practical equation for the coarse alignment
33 Attitude Fine Alignment According to the sensor errorlevel about 200 samples (119872 asymp 200) of DVL data are neededfor the coarse alignment Since the update rate of the DVL isseveral Hz the coarse alignment could cost about 60 s After60 s of the attitude coarse alignment the pitch and roll wouldattain about 01 deg accuracy and the heading error would bearound 10 deg The fine alignment starts immediately after itThe goal is to improve the attitude accuracy especially theheading accuracy
For the fine alignment most methods use the SINSattitude and velocity error models as the state transitionequation for the data fusion algorithms such as the Kalman
filter (KF) the extended KF (EKF) and the UKF The UKFfor its worthy performance in nonlinear estimation [19 20]is especially used for nonlinear misalignment angles Hereis the review of the UKF based attitude fine alignment withnonlinear misalignment angles model [7 21 22]
The navigation algorithm is described in (1) (2) and (3)The state of the UKF is
x = [120575k119899 120579 120575120596119887
119894119887120575f119887]119879
(28)
And the large misalignment angles model is
120575k119899 = (I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899
minus (2119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
120579 = (I minus 119862
1198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
120575119887
119894119887= 0 + 120576
120596 120575
f119887 = 0 + 120576119891
(29)
where 119910 is the real data equal to the ideal data 119910 plus the error120575119910 119862119899
1015840
119887is the DCM from the 119887 frame to the 1198991015840 frame 120576
120596and
120576119891are the noise of the gyroscopes and accelerometers and
1198621198991015840
119899is the DCM from the 119899 frame to the 119899
1015840 frame Assumethat the 119899 frame to the 1198991015840 frame is completed through threenoninterchangeable rotations 120579
119911 120579119909 and 120579
119910 Each rotation
can be expressed as
119862120579119909
=[
[
[
1 0 0
0 cos 120579119909
sin 120579119909
0 minus sin 120579119909
cos 120579119909
]
]
]
119862120579119910
=
[
[
[
[
cos 120579119910
0 minus sin 120579119910
0 1 0
sin 120579119910
0 cos 120579119910
]
]
]
]
119862120579119911
=[
[
[
cos 120579119911
sin 120579119911
0
minus sin 120579119911
cos 120579119911
0
0 0 1
]
]
]
(30)
The total DCM is
1198621198991015840
119899= 119862120579119910119862120579119909119862120579119911 (31)
The measurement model is
z = k119899 minus 119862119899
11989910158401198621198991015840
119887k119887DVL = k119899 + 120575k119899
minus 119862119899
11989910158401198621198991015840
119887(k119887DVL + 120575k119887DVL)
= k119899 minus k119899DVL + 120575k119899 minus 120575k119899DVL = 120575k119899 minus 120575k119899DVL
(32)
where 120575k119899DVL is the noise of the DVLThen according to (29) and (32) the nonlinear state and
measurement equations are
Mathematical Problems in Engineering 5
x =
[
[
[
[
[
[
[
[
[
120575k119899
120579
120575119887
119894119887
120575f119887
]
]
]
]
]
]
]
]
]
= 119891 (x) + w
=
[
[
[
[
[
[
[
[
(I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899 minus (2
119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
(I minus 1198621198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
0
0
]
]
]
]
]
]
]
]
+
[
[
[
[
0
0
120576120596
120576119891
]
]
]
]
z = ℎ (x) + u = [120575k119899] + [minus120575k119899DVL]
(33)
where w = [03times1 03times1120576120596120576119891] is the state noise and u =
[minus120575k119899DVL] is the measurement noiseThe fine alignmentrsquos duty is to estimate 119862
119899
1198991015840 to improve
the accuracy of 119862119899119887 Notice that 119862119899(0)
119887(0) 119862119887(0)119887(119905)
and 119862119899(0)
119899(119905)can still
be updated in fine alignment It means that a more precise119862119899(0)
119887(0)is realized too according to the inverse of (10) in the fine
alignment stage as
119862119899(0)
119887(0)= 119862119899(0)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(0) (34)
This result is of great importance in the position alignment
4 Position Alignment
41 Position Alignment Algorithm The original positionupdate equation is described in (3) This position updateequation can also be performed between the 119890 frame and the119887 frame as
119862119899(0)
119899(119905)Rminus1119888p (119905) = 119862
119899(0)
119887(0)119862119887(0)
119887(119905)k119887 (119905) (35)
In order to get the position (35) should be integrated on eachside first The integral form of the left term is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
(36)
where 119905end is the time point where the attitude alignmentends Substitute (11) into (36)
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(37)
The integral part of (37) can be converted as
int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(38)
where 119905119896= 119905end when 119896 = 119873 Practically the effort of the
position change is really small in the integration It could beregarded as constant as
p (119905) = p (119905end) (39)
where p(119905end) is the position which needs alignment Substi-tute (39) into the integral part of (38)
int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
asymp int
119905119896+1
119905119896
(I + (119905 minus 119905119896)120596119899
119894119899times)120596119899
119894119899times Rminus1119888p (119905end) 119889119905
= 120596119899
119894119899times Rminus1119888p (119905end) 119879 + 120596
119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(40)
Therefore the computed form of (36) is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(41)
For (41) 120596119899119894119899times120596119899
119894119899timesRminus1119888p(119905end) 11987922 which is a second-order
term is much smaller than 120596119899119894119899times Rminus1119888p(119905end)119879 with 119879 lt 1 So
6 Mathematical Problems in Engineering
this part could be omittedThen notice that119879120596119899119894119899times asymp C119899(119905119896)
119899(119905119896+1)minus
I (41) can be simplified as
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(C119899(119905119896)119899(119905119896+1)
minus I)Rminus1119888p (119905end)
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus [119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (119905end)]
= Rminus1119888p (119905end) minus Rminus1
119888p (0)
(42)
The integral form of the right term of (35) is
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
= 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
119862
119887(119905119895)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
(I + (int
119905
119905119895
120596119887
119894119887119889120591)times) k119887119889119905
(43)
where 119905119895= 119905end when 119895 = 119872 Similarly v119887 can be approxi-
mated linearly as
k119887 (119905) = k119887 (119905119895) +
119905 minus 119905119895
119879DVL(k119887 (119905
119895+1) minus k119887 (119905
119895)) (44)
where 119879DVL is the duration of two DVL samples Substitute(23) (25) and (44) into (43)
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(45)
According to (35) (42) equals (45) It follows that
p (119905end) = p (0)
+ R119888119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896))
+
1
3
Δ1205791times 119879DVLk
119887(119905119896)
+ (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(46)
For (46) all the data can be calculated in real-time except119862119899(0)
119887(0) It means that when the initial attitude matrix 119862
119899(0)
119887(0)
is obtained the position could immediately be updated Asdescribed in Section 3 119862119899(0)
119887(0)can be gathered by (27) through
the attitude coarse alignment and then the accuracy can beimproved by (34) through the whole attitude fine alignment
Also (39) can be treated as
p (119905) = p (0) (47)
And then (41) goes to
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879 + 120596
119899
119894119899
times120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(48)
And by making (48) be equal to (45) another solution comesout as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(49)
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 5
x =
[
[
[
[
[
[
[
[
[
120575k119899
120579
120575119887
119894119887
120575f119887
]
]
]
]
]
]
]
]
]
= 119891 (x) + w
=
[
[
[
[
[
[
[
[
(I minus 119862119899
1198991015840) 1198621198991015840
119887
f119887 minus (2120575120596119899
119894119890+ 120575120596119899
119890119899) times k119899 minus (2
119899
119894119890+ 119899
119890119899) times 120575k119899 + 119862
119899
11989910158401198621198991015840
119887120575f119887
(I minus 1198621198991015840
119899) 119899
119894119899+ 120575120596119899
119894119899minus 1198621198991015840
119887120575120596119887
119894119887
0
0
]
]
]
]
]
]
]
]
+
[
[
[
[
0
0
120576120596
120576119891
]
]
]
]
z = ℎ (x) + u = [120575k119899] + [minus120575k119899DVL]
(33)
where w = [03times1 03times1120576120596120576119891] is the state noise and u =
[minus120575k119899DVL] is the measurement noiseThe fine alignmentrsquos duty is to estimate 119862
119899
1198991015840 to improve
the accuracy of 119862119899119887 Notice that 119862119899(0)
119887(0) 119862119887(0)119887(119905)
and 119862119899(0)
119899(119905)can still
be updated in fine alignment It means that a more precise119862119899(0)
119887(0)is realized too according to the inverse of (10) in the fine
alignment stage as
119862119899(0)
119887(0)= 119862119899(0)
119899(119905)119862119899(119905)
119887(119905)119862119887(119905)
119887(0) (34)
This result is of great importance in the position alignment
4 Position Alignment
41 Position Alignment Algorithm The original positionupdate equation is described in (3) This position updateequation can also be performed between the 119890 frame and the119887 frame as
119862119899(0)
119899(119905)Rminus1119888p (119905) = 119862
119899(0)
119887(0)119862119887(0)
119887(119905)k119887 (119905) (35)
In order to get the position (35) should be integrated on eachside first The integral form of the left term is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
(36)
where 119905end is the time point where the attitude alignmentends Substitute (11) into (36)
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p (119905) 119889119905
= 119862119899(0)
119899(119905)Rminus1119888p (119905)1003816100381610038161003816
1003816
119905end
0minus int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(37)
The integral part of (37) can be converted as
int
119905end
0
119862119899(0)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
=
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
(38)
where 119905119896= 119905end when 119896 = 119873 Practically the effort of the
position change is really small in the integration It could beregarded as constant as
p (119905) = p (119905end) (39)
where p(119905end) is the position which needs alignment Substi-tute (39) into the integral part of (38)
int
119905119896+1
119905119896
119862119899(119905119896)
119899(119905)120596119899
119894119899times Rminus1119888p (119905) 119889119905
asymp int
119905119896+1
119905119896
(I + (119905 minus 119905119896)120596119899
119894119899times)120596119899
119894119899times Rminus1119888p (119905end) 119889119905
= 120596119899
119894119899times Rminus1119888p (119905end) 119879 + 120596
119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(40)
Therefore the computed form of (36) is
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(41)
For (41) 120596119899119894119899times120596119899
119894119899timesRminus1119888p(119905end) 11987922 which is a second-order
term is much smaller than 120596119899119894119899times Rminus1119888p(119905end)119879 with 119879 lt 1 So
6 Mathematical Problems in Engineering
this part could be omittedThen notice that119879120596119899119894119899times asymp C119899(119905119896)
119899(119905119896+1)minus
I (41) can be simplified as
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(C119899(119905119896)119899(119905119896+1)
minus I)Rminus1119888p (119905end)
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus [119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (119905end)]
= Rminus1119888p (119905end) minus Rminus1
119888p (0)
(42)
The integral form of the right term of (35) is
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
= 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
119862
119887(119905119895)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
(I + (int
119905
119905119895
120596119887
119894119887119889120591)times) k119887119889119905
(43)
where 119905119895= 119905end when 119895 = 119872 Similarly v119887 can be approxi-
mated linearly as
k119887 (119905) = k119887 (119905119895) +
119905 minus 119905119895
119879DVL(k119887 (119905
119895+1) minus k119887 (119905
119895)) (44)
where 119879DVL is the duration of two DVL samples Substitute(23) (25) and (44) into (43)
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(45)
According to (35) (42) equals (45) It follows that
p (119905end) = p (0)
+ R119888119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896))
+
1
3
Δ1205791times 119879DVLk
119887(119905119896)
+ (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(46)
For (46) all the data can be calculated in real-time except119862119899(0)
119887(0) It means that when the initial attitude matrix 119862
119899(0)
119887(0)
is obtained the position could immediately be updated Asdescribed in Section 3 119862119899(0)
119887(0)can be gathered by (27) through
the attitude coarse alignment and then the accuracy can beimproved by (34) through the whole attitude fine alignment
Also (39) can be treated as
p (119905) = p (0) (47)
And then (41) goes to
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879 + 120596
119899
119894119899
times120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(48)
And by making (48) be equal to (45) another solution comesout as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(49)
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
6 Mathematical Problems in Engineering
this part could be omittedThen notice that119879120596119899119894119899times asymp C119899(119905119896)
119899(119905119896+1)minus
I (41) can be simplified as
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)(C119899(119905119896)119899(119905119896+1)
minus I)Rminus1119888p (119905end)
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus [119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (119905end)]
= Rminus1119888p (119905end) minus Rminus1
119888p (0)
(42)
The integral form of the right term of (35) is
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
= 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
119862
119887(119905119895)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)int
119905119895+1
119905119895
(I + (int
119905
119905119895
120596119887
119894119887119889120591)times) k119887119889119905
(43)
where 119905119895= 119905end when 119895 = 119872 Similarly v119887 can be approxi-
mated linearly as
k119887 (119905) = k119887 (119905119895) +
119905 minus 119905119895
119879DVL(k119887 (119905
119895+1) minus k119887 (119905
119895)) (44)
where 119879DVL is the duration of two DVL samples Substitute(23) (25) and (44) into (43)
int
119905end
0
119862119899(0)
119887(0)119862119887(0)
119887(119905)k119887119889119905
asymp 119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(45)
According to (35) (42) equals (45) It follows that
p (119905end) = p (0)
+ R119888119862119899(0)
119887(0)
119872minus1
sum
119895=0
119862119887(0)
119887(119905119895)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896))
+
1
3
Δ1205791times 119879DVLk
119887(119905119896)
+ (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
(46)
For (46) all the data can be calculated in real-time except119862119899(0)
119887(0) It means that when the initial attitude matrix 119862
119899(0)
119887(0)
is obtained the position could immediately be updated Asdescribed in Section 3 119862119899(0)
119887(0)can be gathered by (27) through
the attitude coarse alignment and then the accuracy can beimproved by (34) through the whole attitude fine alignment
Also (39) can be treated as
p (119905) = p (0) (47)
And then (41) goes to
int
119905end
0
119862119899(0)
119899(119905)Rminus1119888p119889119905
asymp 119862119899(0)
119899(119905end)Rminus1119888p (119905end) minus Rminus1
119888p (0)
minus
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879 + 120596
119899
119894119899
times120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(48)
And by making (48) be equal to (45) another solution comesout as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (0) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (0) 119879
2
2
(49)
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 7
And we can also let (41) be equal to (45) to get anothersolution as
119862119899(0)
119899(119905end)Rminus1119888p (119905end)
asymp 119862119899(0)
119887(0)
119873minus1
sum
119896=0
119862119887(0)
119887(119905119896)[
119879DVL2
(k119887 (119905119896+1
) + k119887 (119905119896)) +
1
3
Δ1205791
times 119879DVLk119887(119905119896) + (
1
2
Δ1205791+
1
6
Δ1205792)
times119879DVLk119887(119905119896+1
) ]
+ Rminus1119888p (0) +
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times Rminus1119888p (119905end) 119879
+
119873minus1
sum
119896=0
119862119899(0)
119899(119905119896)120596119899
119894119899times 120596119899
119894119899times Rminus1119888p (119905end)
1198792
2
(50)
Although the expression of (49) and (50) is a little com-plicated the alignment results are almost the same by thethree different approximations This will be approved in thesimulation section
42 In-Motion Alignment Summarization A summarizationof in-motion alignment for DVL aided SINS is proposed togive a better understanding of the whole algorithm
Step 1 Set 119862119899(0)119899(0)
= 119862119887(0)
119887(0)= I p(0) as the beginning position
coordinate
Step 2 Update 119862119899(0)
119899(119905) 119862119887(0)119887(119905)
(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 DVLrsquosdata is available
Step 4 Compute the initial attitude matrix 119862119899(0)
119887(0)based on
(27) Compute the attitude matrix 119862119899(119905)119887(119905)
by (10)
Step 5 Update the position via (46)
Step 6 Go to Step 2 until the attitude coarse alignment iscomplete
Step 7 Use the UKF based fine alignment algorithm Thenavigation algorithm is described in (1) (2) and (3)The stateand measurement equations are described in (33)
Step 8 Update119862119899(0)119899(119905)
119862119887(0)119887(119905)
when the data of inertial sensors isavailable
Step 9 Compute a more accurate 119862119899(0)119887(0)
according to (34)
Step 10 Update the position via (46)
0 10 20 30 40 50 60
0246
Time (s)
Pitc
h er
ror
minus4minus2
times10minus3
(deg
)
(a)
0 10 20 30 40 50 60
024
Time (s)
Roll
erro
r
minus4minus2
minus6
times10minus3
(deg
)
(b)
0 10 20 30 40 50 60
005
Time (s)H
eadi
ng er
ror
Tr1Tr2
Tr3Tr4
minus05
minus1
minus15
(deg
)
(c)
Figure 1 Average attitude errors of four types of trajectories
Step 11 Go to Step 7 until the attitude fine alignment iscomplete
It is clear from these steps that the alignment can work ona real-time SINS and no backtrack is needed
5 Simulation Results
The attitude coarse alignment and position alignment algo-rithms are examined in this section through simulation withfour types of trajectories
The simulation trajectoriesrsquo base latitude and longitudeare 30 deg and 120 deg The pitch roll and heading aresinusoid waves with random amplitude and frequency within10 deg and 1Hz Other parameters are listed below
trajectory 1 (Tr1) zero velocity conditiontrajectory 2 (Tr2) random sinusoid wave conditionwith max velocity about 1mstrajectory 3 (Tr3) line conditionwith randomvelocityaround 14mstrajectory 4 (Tr4) line condition with random veloc-ity around 14ms
The first simulation uses error-free gyroscopes acceler-ometers and DVL data Each type runs 50 times Figure 1shows the average attitude errors The pitch and roll errorsreach 0003 deg within 20 s for all four trajectories And theheading errors reduce to 005 deg for Tr1 and Tr2 after 30 s
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
8 Mathematical Problems in Engineering
0 10 20 30 40 50 60
0
05
Time (s)
Hea
ding
erro
r (de
g)
Tr4
minus1
minus15
minus05
Tr4 no
Figure 2 Average heading errors of Tr4 and Tr4 with the omittedparts kept (Tr4 no)
0 10 20 30 40 50 60
0
005
Time (s)
Pitc
h er
ror
minus005
(deg
)
(a)
0 10 20 30 40 50 60
0
005
Time (s)
Roll
erro
r
minus005
(deg
)
(b)
0 10 20 30 40 50 60
0
20
Time (s)
Hea
ding
erro
r
Tr3
minus20
(deg
)
Tr3 SE
(c)
Figure 3 Average attitude errors of Tr3 and Tr3 with sensor errors(Tr3 SE) Dashed lines are the 3120590 envelopes
alignment And the heading errors grow to 01 deg for Tr3 and1 deg for Tr4 This is mainly because that with the velocityincreasing the omitted parts are growing too And these partsare affecting the heading alignment accuracy
Figure 2 gives the results where the omitted parts areincluded It is clear that the heading accuracy increased Wehope that the alignment algorithm is perfect with all partsbeing considered But the omitted parts are unknown sincethese parts need velocity in the 119899 frame And for the realsensors the sensor errors would affect the accuracy muchmore than the omitted parts
Figure 3 illustrates the alignment errors with sen-sor errors (gyroscopes drift 02 degh gyroscopes noise
0 10 20 30 40 50 600
0005001
Time (s)
Tr1
(m)
(a)
0 10 20 30 40 50 600
0204
Time (s)
Tr2
(m)
(b)
0 10 20 30 40 50 60
05
Time (s)
Tr3
(m)
minus5
(c)
0 10 20 30 40 50 60
050
Time (s)Tr
4 (m
)
Ap1Ap2Ap3
minus50
(d)
Figure 4 Average position errors of four types of trajectories withthree different position approximate methods
09 deghradicHz accelerometers bias 1 times 10minus4 g accelerome-
ters noise 2 times 10minus4 gradicHz DVL bias 15 of voyage velocity
and DVL noise standard variance 002ms) The pitch androll errors are almost the same compared with the error-freesensors while the heading errorrsquos 3120590 envelope is 10 timeslarger than the error-free sensors And it is also larger than theheading error of Tr4 So the largest attitude coarse alignmenterror source is the sensor errors Meanwhile owing to thedifference of the sensor error level the convergence speed ofTr3 SE is slower than Tr3 But the heading error of Tr3 SEcan still be less than 20 deg after 60 s It is enough for the finealignment stage
For the position alignment three different positionapproximate methods are applied in Section 4
Approximation 1 (Ap1) regard p(119905) as p(119905end) and120596119899
119894119899times 120596119899
119894119899times Rminus1119888p(119905end)11987922 part is omitted
Approximation 2 (Ap 2) regard p(119905) as p(0)Approximation 3 (Ap 3) regard p(119905) as p(119905end)
Figure 4 displays the average position errors of these threedifferent approximations The simulation still runs 50 timeswith four types of trajectories
As shown in Figure 4 the position errors of three differentapproximations are almost the same It means that theapproximation for (38) is reasonableThe position errors have
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 9
0 10 20 30 40 50 60
0
20
40
Time (s)
Tr3
(m)
Ap1
minus40
minus20
Ap1 SE
Figure 5 Average position errors of Tr3 with Ap1 and Ap1 withsensor errors (Ap1 SE) Dashed lines are the 3120590 envelopes
GPS
INS
DVL
Figure 6 The towed body with an INS a DVL and a GPS
a significant increase and decrease process in the first 20 sthis is mainly because the accuracy of the initial attitudeDCM 119862
119899(0)
119887(0)is increasing as described in Section 33 The max
position errors of Tr4 are larger than the other trajectories forthe reason that the velocity is at least 10 times larger and theattitude error is 10 times larger too And the accuracy of Ap1is especially a little bit higher than Ap2 and Ap3 So Ap1 isapplied in the next simulation and experiments
Figure 5 demonstrates the average position errors withsensor errors The position errors significantly increase Stillthe largest error source is the sensor errors
6 Experiment Results
61 Experiment Setup The in-motion alignment experimentdata was collected in the Thousand Island Lake in ZhejiangProvince in China by using a towed body shown in Figure 6The sensors used for the experiment are listed below
(1) An inertial navigation system developed by ZhejiangUniversity with four fiber optics gyroscopes (biasinstability 02 degh) and four quartz accelerometers(bias 1 times 10
minus4 g) The data output rate is 500Hz
(2) A 600 kHz Phased Array ExplorerDVL produced byRD Instruments Owing to the export restrictions toChina the accuracy is limited to about 15 of voyageThe data output rate is 4sim5Hz
119055 11906 119065 1190729594
29596
29598
296
29602
29604
29606
29608
Longitude (deg)
Latit
ude (
deg)
Whole voyageAlignment 1Alignment 2
Alignment 3Alignment 4
Figure 7 The route of the towed body
(3) AnOEMV-1GGPS receiver produced byNovatelTheposition accuracy is about 3m The velocity accuracyis about 01ms The data output rate is 5Hz
The GPS is fixed on the top of the towed body while theINS and DVL are installed under water for about 03 m Thetime delay and the lever arm of the sensors are compensated
62 In-Motion Alignment Results The trajectory of the towedbody is shown in Figure 7 Four alignments were done inthe whole voyage Each test includes 60 s coarse alignment240 s fine alignment and 300 s navigationTheGPS aided INSwas already aligned before the experiment and was workingat navigation mode to produce standard position velocityand attitude It was set as a standard system for the resultscomparison
The attitude alignment results are presented in Figures 89 and 10The heading error reaches 20 deg within maximum52 s (alignment 2) andminimum24 s (alignment 3) and oscil-lates in this range during the coarse alignment The headingerror at 60 s is minus52 deg 137 deg 48 deg and 58 deg Thisis consistent with the simulation results shown in Figure 3The convergent speed of the coarse alignment is fast butowing to the lack of the optimization approachmethodwhichcannot handle the noises of the sensors separately the errorjust oscillates This is why the fine alignment is requiredWith heading error of this level it costs minimum 152 s(alignment 1) and maximum 225 s (alignment 3) to reach1 deg of accuracy in the fine alignment stage The errors ofpitch and roll are less than 005 deg after the coarse alignmentand keep in this range So we believe that the total alignmentcould finish within 300 s
The position alignment results are presented in Figures 11and 12 The towed body voyaged at the speed of about 1msThe total voyage distance for each test was about 600m The
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
10 Mathematical Problems in Engineering
0 100 200 300 400 500 600
0
200
Time (s)
Hea
ding
erro
r
Fine alignmentfinish line
Coarse alignmentfinish line
minus200
(deg
)
(a)
0 10 20 30 40 50 60
02040
Time (s)
Hea
ding
erro
r
minus20
(deg
)
(b)
100 150 200 250 300 350 400 450 500 550 600
0
2
Time (s)
Hea
ding
erro
r
minus2
(deg
)
Alignment 1Alignment 2
Alignment 3Alignment 4
(c)
Figure 8 The heading error of the alignment (a) Overview (b)Details of the coarse alignment (c) Details of the fine alignment
0 100 200 300 400 500 600
0
005
01
Time (s)
Pitc
h er
ror (
deg)
Fine alignmentfinish line
Coarse alignmentfinish line
minus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 9 The pitch error of the alignment
error of the four alignments is 3m 16m 7m and 5m afterthe coarse alignment This is consistent with the simulationresults shown in Figure 5 The error reduced to 1m 4m2m and 3m after the fine alignment It is less than 15 ofthe voyage distance The position error grows slowly afterthe alignment This is another evidence to prove that thealignment is effective
Figures 13 and 14 choose alignment 4 to show a compar-ison between the algorithm proposed in this paper (IMA1)and the previous UKF based method reviewed in Section 33(IMA2) As described in [7] the UKF based method couldsolve the attitude alignment problem if the initial heading
0 100 200 300 400 500 600
0
005
01
Time (s)
Roll
erro
r (de
g)
Coarse alignmentfinish line
Fine alignmentfinish lineminus005
minus01
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 10 The roll error of the alignments
0 100 200 300 400 500 6000
1020304050
Time (s)
Posit
ion
erro
r (m
)
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 11 Position error of the alignments
0 100 200 300 400 500 6000
001
002
003
004
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
Coarse alignmentfinish line
Fine alignmentfinish line
Alignment 1Alignment 2
Alignment 3Alignment 4
Figure 12 Position errorvoyage distance of the alignments
error is within about 100 deg But if without the coarsealignment process the heading error is unknown So theinitial 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) theheading error after the alignment is at the same level asIMA1 while if the initial heading error is large (IMA2-150)the heading may appear to be larger than 1 deg after thealignment without the attitude coarse alignment process
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600
0
05
1
15
2
25
3
Time (s)
Hea
ding
erro
r (de
g)
IMA1IMA2-150IMA2-20
minus05
minus1
Figure 13The comparison of the heading error of IMA1 and IMA2
0 100 200 300 400 500 600
0
005
01
Time (s)
Posit
ion
erro
rvo
yage
dist
ance
IMA1IMA2-150IMA2-20
Figure 14The comparison of the position error of IMA1 and IMA2
Also the position error is larger than 6 of the voyagedistance which is 3 times larger than IMA1
7 Conclusions
The accuracy and applicability of SINS largely dependedon the precision and swiftness of the alignment It is adifficult problem to align an in-motion SINSwith underwaterapplications due to the lack of the GPS which could providevelocity in the 119899 frame and position in the 119890 frame Thealignment problem for underwater applications is not onlythe attitude alignment but also the position alignment
This paper proposed a complete alignment solution forDVL aided SINS in-motion by using the integration form ofthe velocity update equation in body frame to give a roughinitial angle for the UKF based fine alignment and using theintegration form of position update equation to alignment ofthe position Simulation and on-lake tests were carried outto verify the alignment The results show that the headingcould reach around 10 deg accuracy and the pitch and rollcould be aligned up to 005 deg during the coarse alignment
The heading could reach 1 deg accuracy in 240 s using UKFbased fine alignment with heading error of this level And theposition error could achieve 15 of the voyage distance
Conflict of Interests
The authors declare that there is no conflict of interests re-garding the publication of this paper
Acknowledgment
This work was supported by The Principal Fund of ZhejiangUniversity
References
[1] W Alameda Jr ldquoSeadevilmdasha totally integrated inertial naviga-tion system (INS) solutionrdquo in Proceedings of the UnderwaterIntervention Symposium 2002
[2] B Jalving K Gade K Svartveit A Willumsen and RSoslashrhagen ldquoDVL velocity aiding in the HUGIN 1000 integratedinertial navigation systemrdquo Modeling Identification and Con-trol vol 25 no 4 pp 223ndash235 2004
[3] Oslash Hegrenaeligs and E Berglund ldquoDoppler water-track aidedinertial navigation for autonomous underwater vehiclerdquo inProceedings of the IEEE Bremen Balancing Technology withFuture Needs (OCEANS rsquo09) May 2009
[4] S Hong M H Lee H S Kwon and H H Chun ldquoA car test forthe estimation ofGPSINS alignment errorsrdquo IEEETransactionson Intelligent Transportation Systems vol 5 no 3 pp 208ndash2172004
[5] S Han and J Wang ldquoA novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applicationsrdquo Journal ofNavigation vol 63 no 4 pp 663ndash680 2010
[6] D Titterton and J Weston Strapdown Inertial Navigation Tech-nology The Institution of Electronical Engineers RestonVaUSA 2nd edition 2004
[7] W Li J Wang L Lu and W Wu ldquoA novel scheme forDVL-aided SINS in-motion alignment using UKF techniquesrdquoSensors vol 13 no 1 pp 1046ndash1063 2013
[8] J Cheng and F J de Wan ldquoA fast initial alignment method forstrapdown inertial navigation system on stationary baserdquo IEEETransactions on Aerospace and Electronic Systems vol 32 no 4pp 1501ndash1505 1996
[9] L Schimelevich and R Naor ldquoNew approach to coarse align-mentrdquo in Proceedings of the IEEE Position Location and Naviga-tion Symposium (PLANS rsquo96) pp 324ndash327 April 1996
[10] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactionson Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011
[11] YWu and X Pan ldquoVelocityposition integration formula part Iapplication to in-flight coarse alignmentrdquo IEEE Transactions onAerospace and Electronic Systems vol 49 no 2 pp 1006ndash10232013
[12] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 1 attitude algorithmsrdquo Journal of GuidanceControl and Dynamics vol 21 no 1 pp 19ndash28 1998
[13] P G Savage ldquoStrapdown inertial navigation integration algo-rithm design part 2 velocity and position algorithmsrdquo Journal
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
12 Mathematical Problems in Engineering
of Guidance Control and Dynamics vol 21 no 2 pp 208ndash2211998
[14] P G Savage StrapdownAnalytics StrapdownAssociatesMaplePlain Minn USA 2000
[15] Y Wu and X Pan ldquoVelocityposition integration formula partII application to strapdown inertial navigation computationrdquoIEEE Transactions on Aerospace and Electronic Systems vol 49no 2 pp 1024ndash1034 2013
[16] M D Shuster and S D Oh ldquoThree-axis attitude determinationfrom vector observationsrdquo Journal of Guidance Control andDynamics vol 4 no 1 pp 70ndash77 1981
[17] I Y Bar-Itzhack ldquoREQUEST a recursive QUEST algorithmfor sequential attitude determinationrdquo Journal of GuidanceControl and Dynamics vol 19 no 5 pp 1034ndash1038 1996
[18] P G Savage ldquoConing algorithm design by explicit frequencyshapingrdquo Journal of Guidance Control and Dynamics vol 33no 4 pp 1123ndash1132 2010
[19] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoNewapproach for filtering nonlinear systemsrdquo in Proceedings of theAmerican Control Conference pp 1628ndash1632 June 1995
[20] E A Wan and R van der Merwe ldquoThe unscented Kalman filterfor nonlinear estimationrdquo in Proceedings of the IEEE AdaptiveSystems for Signal Processing Communications and ControlSymposium (AS-SPCC rsquo00) pp 153ndash158 2000
[21] X Kong E M Nebot and H Durrant-Whyte ldquoDevelopmentof a non-linear psi-angle model for large misalignment errorsand its application in INS alignment and calibrationrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo99) pp 1430ndash1435 May 1999
[22] H S Hong J G Lee and C G Park ldquoPerformance improve-ment of in-flight alignment for autonomous vehicle underlarge initial heading errorrdquo IEE Proceedings Radar Sonar andNavigation vol 151 no 1 pp 57ndash62 2004
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
