Research Article Error Analysis and Compensation of Gyrocompass Alignment for …downloads.hindawi.com/journals/mpe/2014/373575.pdf · 2019-07-31 · the gyrocompass alignment and

Research ArticleError Analysis and Compensation of GyrocompassAlignment for SINS on Moving Base

Bo Xu1 Yang Liu1 Wei Shan1 Yi Zhang23 and Guochen Wang1

1 Harbin Engineering University 145 Nantong Road Harbin 150001 China2 Beijing Aerospace Automatic Control Institute No 50 Yongding Road Haidian District Beijing 100039 China3National Key Laboratory of Science and Technology on Aerospace Intelligence Control Beijing 100854 China

Correspondence should be addressed to Bo Xu xubocartersinacom

Received 29 January 2014 Accepted 24 May 2014 Published 25 June 2014

Academic Editor Bin Jiang

Copyright copy 2014 Bo Xu 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

An improved method of gyrocompass alignment for strap-down inertial navigation system (SINS) on moving base assisted withDoppler velocity log (DVL) is proposed in this paper After analyzing the classical gyrocompass alignment principle on static baseimplementation of compass alignment on moving base is given in detail Furthermore based on analysis of velocity error latitudeerror and acceleration error on moving base two improvements are introduced to ensure alignment accuracy and speed (1) thesystem parameters are redesigned to decrease the acceleration interference and (2) a data repeated calculation algorithm is usedin order to shorten the prolonged alignment time caused by changes in parameters Simulation and test results indicate that theimproved method can realize the alignment on moving base quickly and effectively

1 Introduction

Initial alignment is the process of determining the axesorientation of strap-down inertial navigation system (SINS)with respect to the reference navigational frame To meet thequick response of ships and enhance survivability alignmenton moving base has become a key technique for SINS [1]Different from the alignment for SINS on static base externalinformation should be brought in to assist alignment for SINSon moving base [2] At present the research of alignment onmoving base mainly focuses on the assist of GPS locationHowever GPS systemmay have some restrictions in practicalapplication [3ndash5] Compared with GPS Doppler velocity log(DVL) is an underwater available independent and highaccuracy velocity measuring element commonly used onships and the research of initial alignment for SINS onmoving base assisted with DVL has attracted much moreattention [6]

The initial alignment methods on moving base canbe commonly classified into three mainstream directionstransfer alignment integrated alignment and gyrocompassalignment In transfer alignment by means of velocity

matching and attitude matching a misaligned slave inertialnavigation system can be aligned with the assistance of amaster inertial navigation system It can accomplish theinitial alignment quickly and accurately but the overallsystem is very complex [7] Integrated alignment is an initialalignment method based on modern estimation theory andstate space description It can accomplish alignment rapidlyand precisely by using modern filtering methods to estimatethemisalignment angleHowever it is difficult to establish theabsolutely accurate mathematical model and noise model ofthe system and the large amount of calculation in alignmentprocess always leads to poor instantaneity [8] Gyrocompassalignment is built on the basis of classical control theory sothere is no need to establish accurate mathematical modeland noise model Its algorithm is simple and the calculationamount is greatly reduced However as its fundamentals areestablished on static base or quasi-static base when appliedonmoving base gyrocompass alignmentwill be inaccurate oreven impracticable with the effect of speed and acceleration

In recent years to improve the performance of initialalignment for SINS on moving base gyrocompass alignmentmethods have already been analyzed by some researchers

Hindawi Publishing CorporationMathematical Problems in EngineeringVolume 2014 Article ID 373575 18 pageshttpdxdoiorg1011552014373575

2 Mathematical Problems in Engineering

In paper [9] azimuth axis rotating is used to improve theaccuracy of compass loop but the paper does not expandit to moving base [9] Based on the principle of strap-downgyrocompass alignment Cheng Xianghong from Southeast-ern University points out that the carrier velocity can affectthe gyrocompass alignment and puts forward a calibrationmethod for the inertial sensors of the SINS in the processof alignment on moving base [10] Yan Gongmin fromNorthwestern Polytechnical University proposes a calculatemethod applied to the strap-down gyrocompass alignmenton moving base [11] As gyrocompass alignment needs lesscalculation amount but still remains reliable it has a greatapplication prospect for marine SINS alignment Hwanget al conduct precalibration using dual-electric compasses tominimize the error of spreader pose control [12]

However few papers analyzed the problems faced by thegyrocompass alignment on moving base in a systemic andcomprehensive way Aiming at this problem after analyzingthe principle of classical gyrocompass initial alignment thispaper put forward a gyrocompass alignmentmethod for SINSon moving base aided with Doppler velocity log (DVL) anddeduces the process of algorithm realization in detail Basedon the characteristics of DVLrsquos measuring error we analyzethe influence of velocity error of gyrocompass alignmenton moving base and then establish a misalignment anglemodel It can be found from the analysis that the mostsevere interference comes from the acceleration The systemparameters can be redesigned to restrain this kind of errorbut it can also cause time growth To shorten the gyrocompassinitial alignment time a data repeated calculation algorithmis also introduced [13]

The paper is organized as follows In Section 2 theprinciple and realization of classical gyrocompass alignmentfor SINS on static base are introduced and the systemcharacteristic is analyzed and in the end it leads to gyrocom-pass alignment on moving base In Section 3 a DVL aidedcompass alignment method on moving base is proposedThe effect of velocity error latitude error and accelera-tion interference are analyzed respectively and the systemparameters are redesigned to reduce the most serious effectIn Section 4 a fast compass alignment method based onreversed navigation algorithm is put forward and the detailedcalculating equations are given In Section 5 simulationsabout the alignment methods mentioned above are done InSection 6 a lake test with a certain type of SINS is carried outFinally conclusions are drawn in Section 7

2 Gyrocompass Alignmentfor SINS on Static Base

21 Gyrocompass Alignment Principle Gyrocompass align-ment is commonly used in many kinds of inertial navigationsystems [13ndash15] based on compass effect principle Thisazimuth alignment method is proceeded after horizontalleveling adjust in application By using control theory andadding dampings it can make the platform coordinateapproach the navigation coordinate gradually In this sectionthe operating principle and implementation of gyrocompass

yt yp

120601z 120601z

xt

xp

Ωcos120593

minussin120601zΩcos120593

Figure 1 Schematic diagram of gyrocompass effect

alignment for SINS on static base are described and theaccuracy on static base is also analyzed

Compared with eastern horizontal loop northern hori-zontal loop has an extra coupling term120601

119911120596119894119890cos120593 which is in

proportional relationship with the azimuth error angle Thistermhas the same functionwith eastern gyro drift and it is anangular rate that comes from projection of the earth rotationangular rate in essence [16] When there is an azimuth errorangle 120601

119911between the platform coordinate system and the

geographic coordinate system the northern earth rotationangular rate Ω cos120593 will partly be projected to the platformcoordinate system in eastern axis and its projection value isminus sin120601

119911120596119894119890cos120593 After coarse alignment the projection value

can approximately be simplified to minus120601119911120596119894119890cos120593 Then the

azimuth error angle can be coupled to northern horizontalloop by term minus120601

119911120596119894119890cos120593 This coupling relationship is

defined as gyrocompass effect as shown in Figure 1 [17]Due to gyrocompass effect the horizontal error angle 120601

119909

is influenced additionally by the effect of azimuth error angle120601119911 and the projection value of gravity acceleration along

northern axis in platform coordinate system will change Itwill lead to the change of velocity error in the northern loopMaking use of this coupling relationship the gyrocompassalignment method controls the up axis gyro with the velocityerror information and forms a new closed loop circuits calledthe gyrocompass loop [18] Reasonable designed gyrocom-pass circuit parameters can make the system stable fast andmore accurate thus the gyrocompass alignment process canbe accomplished

22 Realization of Gyrocompass Alignmentfor SINS on Static Base

221 System Realization The direction cosine matrix 119862119901119887

from carrier coordinate system to the platform coordinatesystem is an important matrix in the process of calculationThe angular velocity and acceleration information measuredby IMU must be transformed into platform coordinate

Mathematical Problems in Engineering 3

fb

fp = Cp

bfb

Cp

b

Cp

b

Cp

b

120596bc = Cp

b120596pc

120596pc

fp

120596bip

120596bib

120596pie

120596pie

120596pep

120596bip = Cbp( + 120596pep)

Attitude

Calculation of therevised angular rate

120596bc minus

minus

Cp

b = Cp

btimes 120596bpb

Figure 2 Schematic diagram of gyrocompass alignment of SINS

system via matrix 119862119901119887before participating in the navigation

process While 119862119901119887has the same function with the physical

platform of SINS it is also called the mathematical platformAs the mathematical platform is used instead of the phys-

ical platform in SINS119862119901119887has become the control object of the

revised angular velocity in gyrocompass method principleThe updating algorithm of the mathematical platform 119862

119901

119887is

as follows

120596119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890)

119901

119887= 119862119901

119887times 120596119887

119901119887

(1)

The angular velocity to controlmathematical platform119862119901

119887

is120596119887119901119887 Considering the drift error of gyro and the error caused

by interference movement of carrier the revised angularvelocity of SINS is added After adding the control angularvelocity 120596119901

119888 the corresponding mathematical platform of

SINS control equation is as follows

120596119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888) (2)

The schematic diagram of gyrocompass initial alignmentof SINS is given in Figure 2

222 Calculation of the Revised Angular Rate The revisedangular rate can be obtained as shown in Figure 2 Figures 3and 4 are the north channel and azimuth channel respec-tively in the compass alignment loop

In Figure 3 as the dash line shows 1198701is a damping term

used to decrease the oscillation amplitude of Schuler loop asthe dash-dot line shows119870

2is applied to shorten the systemrsquos

natural period of oscillating period by radic1 + 1198702times After

term 120596119901119888119909there is a horizontal angle error caused by gyro drift

and azimuthmisalignment angle As the double dash-dot lineshows 119870

3is an energy storage term introduced to offset this

error All the 119870 values above can be calculated by dampedcoefficient 120585 and time constant 120590

1198701= 3120590 119870

2= (2 +

1

1205852)1205902

1205962119904

minus 1 1198703=

1205903

12058521205962119904

120596119904= radic

119877

119892 120590 = 120585120596

119899

(3)

Compared with Figure 3 1198703is replaced by 119870(119904) in

Figure 4 to reflect the compass effect term 119870(119904) =1198703119877120596119894119890cos120593sdot(119904+119870

4) and its purpose is to reduce the azimuth

angle 120601119911to an allowed range All the119870 values above can also

be calculated by damped coefficient 120585 and time constant 120590

1198701= 1198703= 2120590 119870

2=1205902 + 1205962

119899

1205852 sdot 1205962119904

minus 1 1198704=

41205904

1205852 sdot 1205962119904

120596119904= radic

119877

119892

(4)The alignment accuracy on static base is mainly decided

by eastern and northern accelerometer zero bias nabla119864 nabla119873and

eastern gyro drift 120576119864

120601119904119909= minus

1

119892nabla119873 (5)

120601119904119910=1

119892nabla119864 (6)

120601119904119911=

120576119864

120596119894119890cos120593

+1198704(1 + 119870

2) 120576119906

119877 sdot 1198703

(7)

23 Static Base Gyrocompass Circuit Characteristic AnalysisGyrocompass alignment on static base or quasi-static basehas the following characteristics

231 No External Acceleration Effect Gyrocompass align-ment changes the strap-down inertial navigation controlsystem into a stable system in principle However Schulerloop of the system is destroyed and external accelerationimpact is introduced into the system According to Figures 3and 4 although both of the two gyrocompass alignmenthorizontal loopswill be infected by acceleration the influencebrought by motion acceleration can be ignored as the carrieraccelerations 119860

119873and 119860E can be approximately regarded in

this status

232 Dispense with Updating of 120596119901119890119901

and 120596119901119894119890 According to

Figure 2 besides the measured value of gyro and acceleratorthere are inputs 120596119901

119890119901and 120596

119901

119894119890in gyrocompass alignment

realization process In gyrocompass alignment system thereis only attitude calculation but no velocity and positioncalculation so the value of 120596119901

119890119901and 120596119901

119894119890cannot be got except

for bringing in external information As the velocity of carrieris zero and the position of carrier remains the same on staticbase or quasi-static base the value of 120596119901

119890119901and 120596119901

119894119890can be got

directly without updating calculation


K3

s

K2

R

1

R

1

s

K1

+

+ +

+

minus

minus120596pcx

120576nnablan

ΔAN

Figure 3 Schematic diagram of gyrocompass alignment in north channel

K(s)

K2

R

1

R

1

s

K1

+

minus

+

+minus

pcx

nnablan

pcz

ΔAN

Figure 4 Schematic diagram of gyrocompass alignment in azimuthchannel

233 Fixed Instrument Error inGeographyCoordinate SystemSINS is strapped to carrier coordinate system so its instru-ment error is defined in carrier coordinate system Becausethe inertial navigation error equation is established in geogra-phy coordinate system the analysis of instrument error has tobe projected in geography coordinate system As the carriercoordinate system remains relatively unchangeable with thegeography coordinate system on static base the instrumenterror in geography coordinate system is still constant

24 Gyrocompass Alignment on Moving Base Motion ofcarrier will change the relative position inevitably betweengeographical coordinate and inertial space One reason isthat the earthrsquos rotation angular velocity 120596

119894119890will change the

direction of the earth coordinate system in inertial space theother is that the movement of carrier on surface of the earthwill cause relative rotation between geographical coordinatesystem and earth coordinate Assuming the velocity of carrieris 119881 and the azimuth angle is 120595 in carrier coordinatethen their projections along north and east of geographicalcoordinate are119881

119873= 119881sdot cos120595 and119881

119864= 119881sdot sin120595 respectively

As shown in Figure 5 the rotational angular velocity 120596119899119894119899

of the geographical coordinate system relative to the inertialspace can be regarded as sum of the earthrsquos rotational angularvelocity 120596

119894119890and the relative rotational angular velocity 120596119899

119890119899

yV

x

z

O

Oi

R

VE

VN

120593

Rcos120593

pN

Figure 5 The projection in north and east of geographical coordi-nate

between the geographical coordinate and the earth coordi-nate

[

[

120596119909

120596119910

120596119911

]

]

= [

[

0120596119894119890cos120593

120596119894119890sin120593

]

]

+

[[[[[[

[

minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 sdottan120593119877

]]]]]]

]

(8)

The output of gyroscope projected in navigation coordi-nates is 120596119899

119894119887= 120596119899119894119890+ 120576119899 on static base but on moving base it

becomes120596119899119894119887= 120596119899119894119890+120596119899119890119899+120596119899119899119887+120576119899120596119899

119899119887can be regarded as zero

in uniform motion The output of accelerator projected innavigation coordinates is the gravitational acceleration 119891119899 =minus119892119899 + nabla119899 on static base However it becomes 119891119899 = (2120596119899

119894119890+

120596119899119890119899) times 119881119899 minus 119892119899 + nabla119899 in uniform motion [13]To make error analysis of misalignment caused by move-

ment directly is relatively difficult Therefore the angularmotion and the linear motion caused by movement areequivalent to gyro drift 120576119899

119889and zero bias of acceleration nabla119899

119889


Static base

120596nib = 120596nie + 120576n

fn = minusgn + nablan

120596nib = 120596nie + 120596nen + 120576

n = 120596nie + 120576nd + 120576

n

fn = minusgn + (2120596nie + 120596nen) times n + nablan = minusgn + nabland + nablan

Angularmotion

Constantvelocity

Linearmotion

Uniform motion

Figure 6 The equivalent error caused by uniform motion

on static base correspondingly [19] The equivalent error isshown in Figure 6

The equivalent error can be calculated as follows

120576119899

119889= [120576119889119890120576119889119899

120576119889119906]119879

= 120596119899

119890119899

= [minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 tan120593119877

]119879

nabla119899

119889= [

[

nabla119889119890

nabla119889119899

nabla119889119899

]

]

= (2120596119899

119894119890+ 120596119899

119890119899) times 119881119899

=

[[[[[[[

[

2120596119894119890sin120595 sin120593 sdot 119881 +

1198812sin2120595 tan120593119877

minus2120596119894119890cos120595 sin120593 sdot 119881 minus

1198812 sin120595 cos120595 tan120593119877

minus1198812 sin120595 cos120595

119877+ 2120596119894119890cos120593 cos120595 sdot 119881 +

1198812sin2120595119877

]]]]]]]

]

(9)

The final accuracy of misalignment angle along easternnorthern and up orientation directions can be got by addingthe instrument error and errors caused by carrierrsquos motioninto (5) to (7) that can be expressed as follows

120601119904119909= minus

1

119892(nabla119873+ nabla119889119899) (10)

120601119904119910=1

119892(nabla119864+ nabla119889119890) (11)

120601119904119911=120576119864+ 120576119889119890

120596119894119890cos120593

+1198704(1 + 119870

2) (120576119906+ 120576119889119906)

119877 sdot 1198703

(12)

In (12) 120596119894119890is earthrsquos rotational velocity 120593 is latitude of

carrierrsquos position and nabla119864 nabla119873 120576119864 and 120576

119880are the equivalent

gyro drift and equivalent accelerator bias in navigationcoordinate systemThe corresponding solution inmotionwillbe introduced in the following sections

3 DVL Aided Gyrocompass Alignment onMoving Base

31 DVL Aided Gyrocompass Alignment The analysis inSection 2 gives conclusion that the influence factors ofgyrocompass alignment become more complicated when

the complexity of motion rises From the perspective of sys-tem the influencing form of acceleration in motion is similarto accelerometer bias but its input value is much larger thanaccelerometer bias What is more from the perspective ofDVL aided velocity information error becomesmore instablein motion

From the analysis of (9) compared with gyrocompassalignment on static base error compensations are needed infour parts respectively on moving base They are angularvelocity 120596119901

119890119901 earth rotation angular velocity 120596119901

119894119890 harmful

acceleration119861119901 andmotion acceleration caused by seawavesThe value of acceleration is only affected by waves in

uniformmotion so it can be treated as disturbanceThe otherthree parts can be calculated by the following equations

120596119901

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879 (13)

120596119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos120593]119879

(14)

119861119901

= (120596119901

119890119901+ 2120596119901

119894119890) times 119881119901

(15)

It can be found that the precise information of carrierrsquosvelocity and position is needed in compensation calculationAs the information cannot be obtained in gyrocompassalignment process external information is essential to com-plete the calculation If the initial position is known withthe assistance of DVL velocity information the dynamiccompensation can be calculated by the following methodsafter coarse alignment

311 Velocity Projection Calculation The velocity measuredby DVL is 119881119887 in carrier coordinate system but the velocityin navigation coordinate system has to be calculated Aftercoarse alignment mathematical platform has been estab-lished and misalignment angle is controlled within a certainrange so the velocity in platform coordinate system can begot by projection calculation of mathematical platform SetDVL measurement velocity as 119881119887dvl and its expression is

119881119901

dvl = 119862119901

119887119881119887

dvl (16)

312 Latitude Calculation After coarse alignment the mis-alignment angle is controlled within a smaller range so


fb

Cp

b

Cp

b

120596pc

120596bip

120596bib

120596pie

R

120596pep

120596bip = Cbp

120596bc = 120596pcCb

p

(120596pie + 120596pep)

120596bc

minus

minus

minus

minus

Cp

b

Cp

b

fp = Cp

bfb

120593

120579

120574

120595

Ap

Bp

Compensationalgorithm

Modificationcalculation

Cp

b = Cp

btimes 120596bpb

Vp

dvl

Vp

dvlVb

dvlVp

dvl = Cp

bVb

dvl

120593 = 1205930 minus intVP

dvlN

Figure 7 DVL aided gyrocompass alignment on moving base

the carrier position can be got by integral calculation of theDVL velocity projection value 119881119901dvl

120593 = 1205930minus int

119881119901

dvl119873119877

(17)

119881119901

dvl119873 is the projection of carrier velocity 119881119901dvl along northin platform coordinate system

313 Compensation Value Calculation We can use latitudeand velocity information to calculate compensation value120596119901

119894119890 120596119901119890119901 and 119861119901 in (13)ndash(15) The implementation principle

scheme of gyrocompass alignment on moving base is shownin Figure 7

32 Error Analysis of DVL Aided Compass Alignment onMoving Base There are still some error factors existing in thecompensation calculation method mentioned in Section 31On one hand as there are errors in compensation calculationprocess the calculation of DVL velocity 119881119901dvl and latitude 120593will be effected accordingly On the other hand the errorcaused by sea waves is regarded as interference and ignoredin compensation calculation So we need to analyze the errorof gyrocompass alignment from three aspects velocity errorlatitude error and acceleration error

321 The Effect of Velocity Error As 119881119901dvl can be calculatedby (16) error factors mainly come from the error of attitudematrix 119862119901

119887and the error of DVL measured velocity 119881119887dvl

In alignment process misalignment angle becomes smallergradually so it is unnecessary to make further analysis of itsinfluence

The velocity 119881119887dvl measured by DVL with constant errorcan be written as

119881119887

dvl = 119881119887

+ Δ119881119887

119888 (18)

Equation (18) can be converted to the platform coordi-nate

119881119901

dvl = 119881119901

+ 119862119901

119887Δ119881119887

119888 (19)

Due to swing of carrier and convergence of misalignmentangle there are some tiny variations in 119862119901

119887 The swing with

small amplitude canmake119862119901119887Δ119881119887119888shake around a constant in

limited range So the error of 119881119901dvl can be regarded as the sumof a constant error and a small high frequency oscillationThecalculation related to velocity is the angular velocity 120596119901

119890119901and

the harmful acceleration 119861119901


(1) The Influence 120596119901119890119901

Calculation Errors As shown in (14)velocity is linear to the angular rate so the error in 120596119901

119890119901by

the effect of speed error 120575119881119864and 120575119881

119873can be expressed as

120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)

Under the influence of this error (2) can be rewritten as(21) on moving base

1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)

After adding gyroscopic drift (21) can be written as

1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)

The comparison of (21) and (22) gives conclusion that theerror of 120596119901

119890119901and gyro drift in carrier coordinate system have

the same influence form to attitude updating calculation If itis considered as equivalent gyro drift 120576

119881119889119890and 120576119881119889119906

the errorwith small high-frequency oscillationwill be restrained by thesystem and constant error is the only thing to be consideredBased on (7) with the effect of constant error the alignmenterror caused by equivalent gyro drift in form (20) is as follows

1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)

As analyzed above east gyro drift minus120575119881119873119877 has greater

influence on azimuth angle than azimuth gyro drift120575119881119864119877 cos120593 so the second part in (23) can be neglected and

(23) can be simplified as

1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)

(2) The Influence of Harmful Acceleration Error As shownin Figure 7 the influence form of harmful acceleration 119861119901 issimilar to119860119901Therefore the influence of harmful accelerationand the accelerometer bias can be written in the same formand the acceleration bias error analysis method can also beused to analyze the influence of harmful acceleration

The projection of errors caused by harmful accelerationalong east and north of the platform is as follows

119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)

With the effect of velocity errors 120575119881119864and 120575119881

119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)

The approximate value of harmful acceleration error canbe got by subtracting (26) from (25) and the result is

120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)

There is little change in latitude so 120593 can be consideredas a constant in alignment process The velocity and its errorcan be considered as sum of constant and high frequencyoscillation As the swing frequency is high the system has aninhibition to this oscillation error so its influence is relativelyweak so (26) can be analyzed as the constant gyro drift Theharmful acceleration errors 120575119861119901

119864and 120575119861119901

119873and the constant

accelerometer bias nabla119861119889119890

nabla119861119889119899

are equivalent so accordingto (6) and (7) error angle can be obtained in (27) with theinfluence of the equivalent accelerometer bias

1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873

minus120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)

Synthesizing themisalignment angles caused by two partsof the velocity error the error equation can be rewritten as

120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)

322The Effect of Latitude Error Latitude calculation can bemainly divided into two parts one is the calculation of 120596119901

119894119890


and 120596119901119890119901

and the other is the calculation of the parametersin feedback loop They are analyzed respectively in thefollowing sections

(1) The Influence Caused by Calculation Error of 120596119901119894119890and 120596119901

119890119901

From (13) and (14) we can know that the calculation of 120596119901119894119890

and 120596119901119890119901

is related to the latitude error Set 1205931015840 as inaccuratelatitude and 1205751205931015840 = 1205931015840 minus 120593 as latitude error

While there exits error in latitude the value of 120596119901119894119890can be

calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)

Therefore the miscalculation of 120596119901119894119890can be got by sub-

tracting (13) from (30)

120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)

While there exists an error the calculation value of 120596119901119890119901is

1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)

The calculation error of 120596119901119890119901can be got by subtracting (14)

from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)

Similar to the analysis of 120575120596119901V119890119901 120575120596119901120593119890119901

and 120575120596119901120593119894119890

can betreated as equivalent gyro drift then combining with (31) and(33) the gyro drift error can bewritten as the sumof 120575120596119901120593

119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0

120596119894119890sin 120575120593 sdot (minus2 sin(120593 +

120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)

From the analysis of (23) as cos120593 is in the denominatorthe error will be infinite in theory when the carrier is sailingin high latitudes However the gyrocompass alignment isapplied in mid or low latitudes so this situation is out ofconsideration [20] As the radius of the earth is very largethe carrierrsquos change in position can only lead to tiny changein latitude and the latitude error 120575120593 is even fainter Deadreckoning latitude error terms 120596

119894119890sin 120575120593 and sin 120575120593119877 in (34)

can be neglected compared with the relatively larger error ofvelocity

(2) The Influence of Corrected Angular Velocity The correctedangle rate of gyrocompass alignment needs to be calculatedthrough the form of Figures 3 and 4 Therefore the values ofparameters 119870

1 1198702 1198703 and 119870

4are needed to be determined

Equation (4) in Section 22 shows that the azimuth loopparameters are calculated as follows

1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)

120585 and 120596119899are adjustable variables in system 120596

119894119890and 119892 are

known values When errors occurred in 120593 the accurate valueof parameter119870(119904) cannot be obtained and the incorrect119870(119904)will affect the convergence speed of the system However asthe value of 120575120593 is small this influence on the convergencespeed is weak

323 The Effect of Acceleration Interference The main accel-eration of carrier is caused by wind and waves in uniformstraight line motion Assuming that acceleration caused bywaves is a sine periodic oscillation and its value is 119860

119901=

119860 sin(120596119905+120593) then the velocity error can be got by integratingthe acceleration It is a cosine periodic oscillation and itsvalue is 119881

119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For

the acceleration interference there is harmful accelerationinterference caused by the velocity error except for 119860119901 andall the above factors can be equivalent to acceleration zerobiases nabla

119860119889119899and nabla

119860119889119890as described in Section 321

nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877

119860119864minus 2120596119894119890cos120595 sin120593 sdot 119881

119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)

There are also equivalent gyro drifts in three directionsproduced by velocity error 120576

119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)

The horizontal alignment is mainly affected by acceler-ation bias and the azimuth alignment is mainly affected byeast gyro drift At this time the misalignment angle equationin the frequency domain can be written as

120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)

The acceleration 119860119875produced by waves is in form of

periodic oscillationwith small amplitude and high frequencyIts input frequency is generally limited in [120596

119899 +infin) so the

effect of 119860119875on misalignment angle can be greatly reduced

by lowering the value of 120596119899 The effect of uniform motion

interference acceleration can be suppressed by changing theparameters in the system but the alignment time will also beincreased accordingly

In this section the error of gyrocompass alignment inuniform straight line motion is analyzed from three aspectslatitude error velocity error and acceleration errorThe influ-ence of velocity error and latitude error on the misalignmentangle is too weak to be considered but the interferenceacceleration brought by swing and waves in motion has agreat influence on the misalignment angles We can reducethis influence by changing the system parameters but thealignment time will be increased accordingly

4 A Rapid Implementation of DVL AidedGyrocompass Method in Alignment

In platform inertial navigation system (PINS) it is difficultfor the platform to revert to former states and adding anew control method again whereas for SINS assuming thatthe storage capacity of navigation computer is large andcomputing power is strong enough it is feasible for thenavigation computer to make a storage of the sampling data

of SINS and calculate the data repeatedly with different kindsof algorithms By using this kind of repeated calculationmethod the increased alignment time caused by changes inparameters can be solved to some extent

There exists a certain convergence in the gyrocompassalignment process and it is themain factor to affect the align-ment time so alignment time can be shortened by reducingthe convergent time or accomplishing the convergence inother processes By calculating the data repeatedly with theconvergence completed in this repeated calculation processthe original alignment process is shortened which in turnreduces the alignment time although the overall convergenceprocess did not change

From the analysis above the conception and structure ofan improved rapid alignment algorithm is given as followsthe gyro and acceleration sampling data of SINS can beregarded as a group of time series The traditional navigationprocess calculates this data series according to time orderand real-time navigation results can be got without the storedprocedure For the same reason if this data series is storedby navigation computer they can be calculated backward toconduct the processing and analyzing procedure as well Itis called data repeated calculation algorithm in this paperand by analyzing the sampling data forward and backwardrepeatedly the accuracy is increased and the actual length ofthe analyzed data series is shortened in return reduced thealignment time The schematic diagram of the data repeatedcalculation algorithm is shown in Figure 8 in whichΔ119879 is thesampling period

Attitude velocity and position calculation of the compassalignment algorithm for SINS are expressed in the followingdifferential equation

119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)

120575V119899 is obtained by compass circuit and 119862119899119887 V119899 =

[V119899119864 V119899119873 V119899119880]119879 120593 and 120582 are inertial attitude matrix speed

latitude and longitude respectively120596119887119894119887and119891119887119894119887aremeasuring

gyro angular velocity and measuring acceleration respec-tively 120596

119894119890and 119892 are the angle rate of the earth and the

local acceleration of gravity respectively 119877 is the radius ofearth Operator 119894(∙times) is the antisymmetric matrix composedby ∙ vector Assuming the sampling period of gyroscope


Normal compass alignment

Reversed compassalignment

Forward compassalignment

Save the sampled data

tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT

Figure 8 Data repeated calculation alignment process diagram

and accelerometer in SINS are both Δ119879 the differentialequations ((39a) (39b) and (39c)) are discrete recursionmethod suitable for computer calculating

119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899

119890119899119896minus1) times V119899119896minus1+119892119899

]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)

120596119899119888= 120575V119899119877 120575V119899 can be obtained by compass circuitFrom the equations above if we take the opposite value

of gyro output and the earth rotation angle rate of theforward navigation algorithm set the initial value of thealgorithm as 119862119899

1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and

0= 120582119898 and calculate the sampling data repeatedly the

repeated calculation algorithm can be achieved It has thesame expression with the forward navigation calculation andthe reversed navigation calculating process from 119905

119898(point B)

to 1199050(point A) can simply be got by using this algorithm

Regardless of calculating error attitude matrix and positioncoordinates are both equal at the same time of the data serieswhile the velocity has the same absolute value with oppositesign in forward and reversed calculation

The reversed navigation algorithm of SINS is as follows

119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899

119890119899119896minus1) times V119899119896minus1

+ 119892119899

]

asymp V119899119896minus Δ119879 sdot [119862

119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1

119877asymp 120593119896minusΔ119879 sdot V119899

119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation

51 Simulation Experiment of Traditional Gyrocompass Align-mentMethod Thecomparison of gyrocompass alignment onstatic base and moving base is given respectively as follows

511 The Simulation Conditions Simulation is proceeded atlatitude 120593 = 457796∘ and longitude 120582 = 1266705∘ (Harbinarea) in order to make a better observation of effect inmotion the triaxial gyro drift of SINS is set as 001∘h andthe bias of accelerator is set as 00001 g The parameters of


Table 1 Misalignments of gyrocompass alignment in differentconditions

Eastern errorangle (∘)

Northern errorangle (∘)

Azimuth errorangle (∘)

In motion minus416 times 10minus3 minus434 times 10minus3 1211Static base 022 times 10minus3 021 times 10minus3 minus0059

Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )

Figure 9 Comparison of the misalignment in east axis

gyrocompass alignment are set as 120585 = 0707 and 120596119899= 0008

it means that the alignment parameters configuration is

1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)

Assuming that the carrierrsquos speed is 10ms and heading isalong 315∘ the swing and sway of sailing are set as sinusoidaloscillation formThe extent of pitch roll and yaw axis swingis set as 6∘ 8∘ and 5∘ and the periods are set as 8 s 6 s and10 s the extent of surge sway and heave is set as 01ms2 andthe periods are 5 s Set the axis misalignment angles of coarsealignment as 01∘ 01∘ and 1∘ respectively

512 The Simulation Results The gyrocompass alignmentmethod is used both on static base and in uniform motionAfter maintaining one hour of alignment process the align-ment results in both conditions are compared and shownin Figures 6ndash8 The thick dash line represents gyrocompassalignment on static base and the thin solid line representsgyrocompass alignment in uniform motion

The error curves in Figures 9 10 and 11 indicate that thegyrocompass alignment has good performance on static basebut there is constant error caused by velocity which existsduring the alignment process while the ship is in motionBy choosing the mean value of alignment errors in twominutes before the alignment process ends the results of bothconditions are recorded in Table 1

In theory three misalignment angles on static base canbe obtained by substituting the gyro drift and acceleratorzero bias into (5)ndash(7) The values are 0209 times 10minus3(∘) 0223 times10minus3(∘) and 0062(∘) respectively which is nearly the same tothe simulation results Then a conclusion can be drawn thatvelocity will cause a sharp alignment error Substitute velocityinto (10)ndash(12) three misalignment angles can be obtained as

Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )

Figure 10 Comparison of the misalignment in north axis

Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1

Figure 11 Comparison of the heading misalignment

0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150

minus20010minus3 10minus2 10minus1 100

Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725

Figure 12 BODE plot of north acceleration to east misalignment

minus42 times 10minus3(∘) minus45 times 10minus3(∘) and 1248(∘) that is nearly thesame with the simulation results as well

On one hand the simulation result proves the perfor-mance of gyrocompass alignment on static base on theother hand it also proves the validity of error analysisfor gyrocompass alignment on moving base discussed inSection 24Therefore the error caused by carrierrsquos movementhas to be amended on moving base

52 Simulation Experiment of DVL Aided GyrocompassAlignment on Moving Base

521The Simulation Conditions Simulation experiments areproceeded in Harbin area where the latitude 120593 = 457796∘


minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483

Figure 13 BODE plot of north acceleration to heading misalign-ment

0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005

Figure 14 Curve of eastmisalignment caused by north acceleration

and the longitude 120582 = 1266705∘ Ignoring all the otherfactors the ship is assumed to sail along northeast directionSet the initial velocity and gyro drift as zero and there existsacceleration on the gyrocompass alignment when the shipis moving The period of acceleration oscillation is 20 s andits value is 119860

119873= 119860119864= 02 sin(2120587 sdot 11990520) Set the axis

misalignment angles of coarse alignment as 05∘ 05∘ and05∘ respectively

522 The Simulation Results We change 119870 value of thehorizontal loop and the azimuth loop in which the dampingratio is still 120585 = 08 while oscillation frequency is adjustedfrom 120596

119899= 002 to 120596

119899= 001 In horizontal loop 119870

1=

00240 1198702= 14769 and 119870

3= 05217 In azimuth loop

1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The

whole simulation time is 3000 s and the simulation diagramsare drawn in Figures 12 13 14 and 15

0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10

Figure 15 Curve of heading misalignment caused by north acceler-ation

Figures 12ndash15 show the BODE figure and correspondingmisalignment curve before and after adjusting system param-eters At frequency of 0314 radsec (the corresponding periodis 20 s) the magnitude is reduced from minus605 dB to minus725 dBin horizontal loop (as shown in Figure 12) Correspondinglythe steady-state oscillation of misalignment is reduced from061015840 to around 0181015840 (as shown in Figure 14) In azimuthloop the magnification is reduced from minus137 dB to minus377 dB(as shown in Figure 13) and the steady-state oscillation ofmisalignment is reduced from 451015840 to around 051015840 (as shownin Figure 15) It can be seen that the influence of accelerationcan be effectively reduced by changing the parameters ofsystem appropriately

However as shown in Figures 12ndash15 alignment time willalso be prolonged accordingly Therefore the data repeatedcalculation algorithm introduced in Section 4 is necessaryand it will efficiently shorten the alignment process

6 Test Verification

61 Test Equipment Set-Up To evaluate the performance ofgyrocompass alignment deeply a sailing test was conductedin testing field on Tai Lake (Wuxi China) The test wasconducted on a high-speed yacht platform equipped withseveral devices It consists of anAHRS based on a fiber opticalgyro (FOG) produced by our own research center (similarto [12] we conducted precalibration process of AHRS tominimize the error) a high-precise FOG-INS system calledPHINS combined with GPS used as a reference system anda DVL used to assist the AHRS system Based on the headingand attitude information supplied by PHINS the accuracy ofgyrocompass initial alignment onmoving basewas evaluatedThe characteristics of AHRS are shown in Table 2 and theperformances of PHINS are shown in Table 3

The high-speed yacht platform used in the test and theset-up of the equipment it carried are shown in Figure 16


AHRS PHINSData collection

computer

UPS power

DVL

GPS

Figure 16 Yacht platform and the test equipment

12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory

Starting point3800 s to 5800 s

Finishing point

Long

itude

Figure 17 Test trajectory

Table 2 The characteristics of AHRS

Gyroscope AccelerometersBias-error 001∘h Threshold plusmn5 times 10minus5 gRandom walkcoefficient lt0005∘radich Bias stability lt1 times 10minus4 g

Scale factorerror lt20 ppm Scale factor

stability lt100 ppm

Measuringrange plusmn55∘s Measuring

range plusmn15 g

62 Test Results andAnalysis The trajectory is across the test-ing field on Tai LakeThe yacht sailed for about 10 kilometersand by recording longitude and latitude of PHINS in realtime we can get the reference path of the yacht Combinedwith the map of testing field trajectory chart of the test isshown in Figure 17

During the test all the original data collected by gyrosand accelerometers and other devices can be saved throughdata collecting system The total time to complete the test is8152 s and 2000 s (from 3800 s to 5800 s) in the test is chosen

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)

Ship motion attitude

minus10

0

10

5

10

15

Time (s)

Figure 18 The shiprsquos heading roll and pitch curves

to conduct an off-line alignment simulation Heading rolland pitch curves of the yacht are shown in Figure 18

Through comparing the initial alignment results withPHINS synchronously we can get the curves of heading andattitude error as shown in Figure 19 (Method 1 corresponds


minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)

Figure 19 The restrain curves of heading and attitude error

to the gyrocompass alignment algorithm on static basewhile Method 2 corresponds to the gyrocompass alignmentalgorithm on moving base)

As shown in Figure 19 the pitch and roll error of gyro-compass alignment algorithm on moving base are of slight

difference compared with algorithm on static base But thereis a significant difference in performance of heading angleon moving base The error of algorithm on moving base isreduced to 15∘ compared with that on static base However itstill can be improved As introduced in Section 323 while


Table 3 The performance of PHINS

Position accuracy (CEP503) Heading accuracy (1120590 value) Attitude accuracy (1120590 value)With stand-alone GPS aiding 5ndash15m With GPS aiding 001∘ secant latitude Roll and pitch error Less than 001∘

With differential GPS aiding 05ndash3mWith RTK differential GPS aiding 2ndash5m No aiding 005∘ secant latitudeNo aiding for 5min 20mPure internal mode 06NMh

minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration

Figure 20 The velocity and acceleration curves under sailingcondition

0 01 02 03 04 050

01

02

03

04

05

06

07

08

09

1Frequency spectrumtimes10minus4

Frequency (Hz)

Am

plitu

deF(j120596)

Figure 21 Frequency spectrogram of acceleration

there exists a periodic interference acceleration in sailingcondition it will produce a periodic oscillation to headingInhibition of acceleration with this periodic oscillation canenhance the alignment performance further

The velocity and acceleration curves of the yacht (from3800 s to 5800 s) are shown in Figure 20

In order to give a clear expression about how the acceler-ation affects the alignment system a fast Fourier transform(FFT) is presented to the acceleration and its frequencyspectrogram is given in Figure 21

Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)

Figure 22 Heading error restrain curves before and after reset

Figure 21 provides a factor that there exists an oscillationperiod of 005Hz in acceleration and it will be equivalentto instrument error which will seriously affect the result ofheading alignment so it is necessary to reset the alignmentparameter of the gyrocompass loop

The convergence curves are compared in Figure 22(Method 2 corresponds to 120596

119899= 005 while Method 3

corresponds to 120596119899= 0007)

Curves in Figure 22 prove that after reset of parametersinitial alignment results are much better than the formerbut the alignment time is significantly increased from about700 s to 1400 s To shorten the prolonged time data repeatedcalculation algorithm is used and the result is shown inFigure 23 In the first 220 s the coarse alignment process iscarried out which can decrease the error angle to certainrange quickly but the precision cannot be guaranteed and theerror vibration that causes a valley at nearly 200 s in Figure 23is obvious After 220 s the proposed alignment method isimplemented after coarse alignment

The chart in the middle represents the data repeated cal-culating process which avoids the sampling step and almosttakes less than 1 s so the alignment time is shortened to about650 s After comparing the alignment curve in Figure 23 withthat in Figure 22 it can be known that the alignment timeis much shorter than before when the accuracy remainsunchanged


0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)

Figure 23 Time of heading alignment after using the data repeated calculation method

Table 4 Statistics of 4 methods

Method 1 Method 2 Method 3 Method 4Roll error(1015840)

Mean 00316 00314 00313 00313Variance minus01623 minus01618 minus01636 minus01636

Pitch error(1015840)

Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)

Mean minus14517 minus03507 minus01327 minus01327Variance 03149 01791 01079 01079

Alignment time (s) 700 700 1400 650

The experimental results of all four methods are puttogether in Table 4 in order to make a comparison Method1 is the gyrocompass alignment method using the algorithmon static base Method 2 is the gyrocompass alignmentmethod using the algorithm on moving base Method 3is the gyrocompass alignment method on moving base inwhich the control parameters are reset and Method 4 isthe improvement of Method 3 after using the data repeatedcalculation algorithm The mean and variance values inTable 4 are the statistics of error angle in the last 20 s ofalignment process (from 1980 s to 2000 s) comparing thealignment results with the standard value collected fromPHINS

From data in Table 4 it can be drawn that Method 4has much better alignment results compared with the otherthree methods That is to say after using the gyrocompassalignment algorithm on moving base with resetting thesystem control parameters the accuracy of initial alignmentis guaranteed and the alignment time is also in acceptablerange with the use of data repeated calculation algorithm

7 Conclusion

Based on the principle analysis of classic platform initial gyro-compass alignment a DVL aided gyrocompass alignmentmethod for SINS on moving base is proposed in this paperThe implementation of algorithm is given and the influenceof external velocity error is also analyzed More specificallytwo methods are adopted to cope with the gyrocompassalignment on moving base first an improved algorithm ofgyrocompass alignment for SINS on moving base aided withDVL is introduced then after the error analysis the systemparameters are reset to decrease the acceleration interferenceHowever from results it turns out that alignment time istoo long to be accepted Aiming at this problem a datarepeated calculation algorithm is put forward to shorten theprolonged time The simulation and experimental resultsverify the performance of the proposed alignment methodboth in accuracy and convergence time

Abbreviations

119877 Earth radius119892 Gravitational acceleration120596119894119890 Rotational angular velocity of the earth

120601119909 120601119910 120601119911 The east north and azimuthmisalignment angle of platform

120593 Latitude120582 Longitude119905 The geographical coordinates119899 The navigation coordinates119901 The platform coordinates119887 The body coordinates119894 The geocentric inertial coordinates119890 The earth coordinates


119862119901

119887 The transform matrix from the platform

frame 119901 to the SINSrsquos body frame 119887120596119887119901119887 Angular rate of the body frame with

respect to the platform frame120596119901119888 The control angular rate

120596119887119894119887 The angular rate of the body frame with

respect to the inertial frame120576 Gyroscopic driftnabla Accelerometer bias120585 The damping coefficient of the system120590 The system constant time120596119899 The system oscillation frequency

120601119904119909 120601119904119910 120601119904119911 The error angle caused by device error in

three directions120595 Heading angle119891119887 Specific force directly measured by the

IMU in the body frame119891119899 Specific force directly measured by the

IMU in the navigation frame119861119901 Bad acceleration120576119889119890 120576119889119899 120576119889119906 The equivalent gyro drift on uniform

motion in three directionsnabla119889119890 nabla119889119899 nabla119889119906 The equivalent accelerometer bias on

uniform motion in three directions119881119887dvl The speed of body measured by DVL120601120575119881119909 120601120575119881119910 120601120575119881119911 The error angle caused in three direc-

tions by uniform motion120576119860119889119890

120576119860119889119899

120576119860119889119906

The equivalent gyro drift in three direc-tions on accelerated movement

nabla119860119889119899

nabla119860119889119890

nabla119860119889119906

The equivalent accelerometer biason accelerated movement in threedirections

120601119860119909 120601119860119910 120601119860119911 The error angle caused by accelerated

movement in three directions

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This study is supported in part by the National NaturalScience Foundation of China (Grant no 61203225) theState Postdoctoral Science Foundation (2012M510083) andthe Central college Fundamental Research Special Fund(no HEUCF110427) The authors would like to thank theanonymous reviewers for their constructive suggestions andinsightful comments

References

[1] E Levinson and C S Giovanni ldquoLaser gyro potential forlong endurance marine navigationrdquo in Proceedings of the IEEEPosition Location and Navigation Symposium pp 115ndash129 Pis-cataway NJ USA December 1980

[2] P M G Silson ldquoCoarse alignment of a shiprsquos strapdown inertialattitude reference system using velocity locirdquo IEEE Transactions

on Instrumentation and Measurement vol 60 no 6 pp 1930ndash1941 2011

[3] J C Yu J B Chen and J H Han ldquoMultiposition observabilityanalysis of strapdown inertial navigation systemrdquoTransaction ofBeijing Institute of Technology vol 24 no 2 pp 150ndash153 2004(Chinese)

[4] X L Wang and G X Sheng ldquoFast and precision multipositioninitial alignment method of inertial navigation systemrdquo Journalof Astronautics vol 23 no 4 pp 81ndash84 2002 (Chinese)

[5] P M Lee B H Jun K Kim J Lee T Aoki and T HyakudomeldquoSimulation of an inertial acoustic navigation systemwith rangeaiding for an autonomous underwater vehiclerdquo IEEE Journal ofOceanic Engineering vol 32 no 2 pp 312ndash345 2007

[6] S S Gao W H Wei Y Zhong and Z Feng ldquoRapid alignmentmethod based on local observability analysis for strapdowninertial navigation systemrdquoActa Astronautica vol 94 no 2 pp790ndash793 2014

[7] T Zhang and X S Xu ldquoA new method of seamless landnavigation for GPSINS integrated systemrdquo Measurement vol45 no 4 pp 691ndash701 2012

[8] X X Liu X S Xu Y T Liu and L H Wang ldquoA fast andhigh-accuracy compass alignmentmethod to sins with azimuthaxis rotationrdquoMathematical Problems in Engineering vol 2013Article ID 524284 12 pages 2013

[9] X H Cheng and M Zheng ldquoOptimization on Kalman filterparameters of SINS during initial alignmentrdquo Journal of ChineseInertial Technology vol 14 no 4 pp 15ndash16 2006 (Chinese)

[10] G M Yan W S Yan and D M Xu ldquoOn reverse navi-gation algorithm and its application to SINS gyro-compassin-movement alignmentrdquo in Proceedings of the 27th ChineseControl Conference (CCC 08) pp 724ndash729 Kunming ChinaJuly 2008

[11] X X Liu X S Xu Y T Liu and L H Wang ldquoA rapid transferalignment method for SINS based on the added backward-forward SINS resolution and data fusionrdquo Mathematical Prob-lems in Engineering vol 2013 Article ID 401794 10 pages 2013

[12] J M Hwang S S Han and J M Lee ldquoApplications of dual-electric compasses to spreader pose controlrdquo InternationalJournal of Control Automation and Systems vol 8 no 2 pp433ndash438 2010

[13] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology Institute of Electrical Engineers Hampshire UK2nd edition 2004

[14] Y Yang and L J Miao ldquoFiber-optic strapdown inertial systemwith sensing cluster continuous rotationrdquo IEEE Transactions onAerospace and Electronic Systems vol 40 no 4 pp 1173ndash11782004

[15] Y N Gao J B Chen and T P Yang ldquoError analysis ofstrapdown optic fiber gyro compassrdquo Transaction of BeijingInstitute of Technology vol 25 no 5 pp 423ndash426 2005

[16] J C Fang and D J 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

[17] X Liu and X Xu ldquoSystem calibration techniques for inertialmeasurement unitsrdquo Journal of Chinese Inertial Technology vol17 no 5 pp 568ndash571 2009 (Chinese)

[18] A Chatfield Fundamentals of High Accuracy Inertial Naviga-tion Institute of Astronautics and Aeronautics Reston VaUSA 1997


[19] Y Li X X Xu and B X Wu ldquoGyrocompass self-alignment ofSINSrdquo Journal of Chinese Inertial Technology vol 16 no 4 pp386ndash389 2008 (Chinese)

[20] R McEwen H Thomas D Weber and F Psota ldquoPerformanceof an AUV navigation system at arctic latitudesrdquo IEEE Journalof Oceanic Engineering vol 30 no 2 pp 443ndash454 2005

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of


Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of


Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of


Mathematical PhysicsAdvances in

Complex AnalysisJournal of


OptimizationJournal of


CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of


Operations ResearchAdvances in

Journal of


Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences


The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014


Algebra

Discrete Dynamics in Nature and Society



Decision SciencesAdvances in

Discrete MathematicsJournal of


Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of


In paper [9] azimuth axis rotating is used to improve theaccuracy of compass loop but the paper does not expandit to moving base [9] Based on the principle of strap-downgyrocompass alignment Cheng Xianghong from Southeast-ern University points out that the carrier velocity can affectthe gyrocompass alignment and puts forward a calibrationmethod for the inertial sensors of the SINS in the processof alignment on moving base [10] Yan Gongmin fromNorthwestern Polytechnical University proposes a calculatemethod applied to the strap-down gyrocompass alignmenton moving base [11] As gyrocompass alignment needs lesscalculation amount but still remains reliable it has a greatapplication prospect for marine SINS alignment Hwanget al conduct precalibration using dual-electric compasses tominimize the error of spreader pose control [12]

However few papers analyzed the problems faced by thegyrocompass alignment on moving base in a systemic andcomprehensive way Aiming at this problem after analyzingthe principle of classical gyrocompass initial alignment thispaper put forward a gyrocompass alignmentmethod for SINSon moving base aided with Doppler velocity log (DVL) anddeduces the process of algorithm realization in detail Basedon the characteristics of DVLrsquos measuring error we analyzethe influence of velocity error of gyrocompass alignmenton moving base and then establish a misalignment anglemodel It can be found from the analysis that the mostsevere interference comes from the acceleration The systemparameters can be redesigned to restrain this kind of errorbut it can also cause time growth To shorten the gyrocompassinitial alignment time a data repeated calculation algorithmis also introduced [13]

The paper is organized as follows In Section 2 theprinciple and realization of classical gyrocompass alignmentfor SINS on static base are introduced and the systemcharacteristic is analyzed and in the end it leads to gyrocom-pass alignment on moving base In Section 3 a DVL aidedcompass alignment method on moving base is proposedThe effect of velocity error latitude error and accelera-tion interference are analyzed respectively and the systemparameters are redesigned to reduce the most serious effectIn Section 4 a fast compass alignment method based onreversed navigation algorithm is put forward and the detailedcalculating equations are given In Section 5 simulationsabout the alignment methods mentioned above are done InSection 6 a lake test with a certain type of SINS is carried outFinally conclusions are drawn in Section 7

2 Gyrocompass Alignmentfor SINS on Static Base

21 Gyrocompass Alignment Principle Gyrocompass align-ment is commonly used in many kinds of inertial navigationsystems [13ndash15] based on compass effect principle Thisazimuth alignment method is proceeded after horizontalleveling adjust in application By using control theory andadding dampings it can make the platform coordinateapproach the navigation coordinate gradually In this sectionthe operating principle and implementation of gyrocompass

yt yp

120601z 120601z

xt

xp

Ωcos120593

minussin120601zΩcos120593

Figure 1 Schematic diagram of gyrocompass effect

alignment for SINS on static base are described and theaccuracy on static base is also analyzed

Compared with eastern horizontal loop northern hori-zontal loop has an extra coupling term120601

119911120596119894119890cos120593 which is in

proportional relationship with the azimuth error angle Thistermhas the same functionwith eastern gyro drift and it is anangular rate that comes from projection of the earth rotationangular rate in essence [16] When there is an azimuth errorangle 120601

119911between the platform coordinate system and the

geographic coordinate system the northern earth rotationangular rate Ω cos120593 will partly be projected to the platformcoordinate system in eastern axis and its projection value isminus sin120601

119911120596119894119890cos120593 After coarse alignment the projection value

can approximately be simplified to minus120601119911120596119894119890cos120593 Then the

azimuth error angle can be coupled to northern horizontalloop by term minus120601

119911120596119894119890cos120593 This coupling relationship is

defined as gyrocompass effect as shown in Figure 1 [17]Due to gyrocompass effect the horizontal error angle 120601

119909

is influenced additionally by the effect of azimuth error angle120601119911 and the projection value of gravity acceleration along

northern axis in platform coordinate system will change Itwill lead to the change of velocity error in the northern loopMaking use of this coupling relationship the gyrocompassalignment method controls the up axis gyro with the velocityerror information and forms a new closed loop circuits calledthe gyrocompass loop [18] Reasonable designed gyrocom-pass circuit parameters can make the system stable fast andmore accurate thus the gyrocompass alignment process canbe accomplished

22 Realization of Gyrocompass Alignmentfor SINS on Static Base

221 System Realization The direction cosine matrix 119862119901119887

from carrier coordinate system to the platform coordinatesystem is an important matrix in the process of calculationThe angular velocity and acceleration information measuredby IMU must be transformed into platform coordinate


fb

fp = Cp

bfb

Cp

b

Cp

b

Cp

b

120596bc = Cp

b120596pc

120596pc

fp

120596bip

120596bib

120596pie

120596pie

120596pep

120596bip = Cbp( + 120596pep)

Attitude


120596bc minus

minus

Cp

b = Cp

btimes 120596bpb







119901

119887is

as follows

120596119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890)

119901

119887= 119862119901

119887times 120596119887

119901119887

(1)


119887





120596119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888) (2)











1198701= 3120590 119870

2= (2 +

1

1205852)1205902

1205962119904

minus 1 1198703=

1205903

12058521205962119904

120596119904= radic

119877

119892 120590 = 120585120596

119899

(3)






1198701= 1198703= 2120590 119870

2=1205902 + 1205962

119899

1205852 sdot 1205962119904

minus 1 1198704=

41205904

1205852 sdot 1205962119904

120596119904= radic

119877

119892




120601119904119909= minus

1

119892nabla119873 (5)

120601119904119910=1

119892nabla119864 (6)

120601119904119911=

120576119864

120596119894119890cos120593

+1198704(1 + 119870

2) 120576119906

119877 sdot 1198703

(7)




this status


and 120596119901119894119890 According to


119890119901and 120596

119901



119890119901and 120596119901



119890119901and 120596119901

119894119890can be got



K3

s

K2

R

1

R

1

s

K1

+

+ +

+

minus

minus120596pcx

120576nnablan

ΔAN


K(s)

K2

R

1

R

1

s

K1

+

minus

+

+minus

pcx

nnablan

pcz

ΔAN






119873= 119881sdot cos120595 and119881





119890119899

yV

x

z

O

Oi

R

VE

VN

120593

Rcos120593

pN



[

[

120596119909

120596119910

120596119911

]

]

= [

[

0120596119894119890cos120593

120596119894119890sin120593

]

]

+

[[[[[[

[

minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 sdottan120593119877

]]]]]]

]

(8)



becomes120596119899119894119887= 120596119899119894119890+120596119899119890119899+120596119899119899119887+120576119899120596119899



119894119890+




119889


Static base

120596nib = 120596nie + 120576n


120596nib = 120596nie + 120596nen + 120576

n = 120596nie + 120576nd + 120576

n


Angularmotion

Constantvelocity

Linearmotion

Uniform motion




120576119899

119889= [120576119889119890120576119889119899

120576119889119906]119879

= 120596119899

119890119899

= [minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 tan120593119877

]119879

nabla119899

119889= [

[

nabla119889119890

nabla119889119899

nabla119889119899

]

]

= (2120596119899

119894119890+ 120596119899

119890119899) times 119881119899

=

[[[[[[[

[

2120596119894119890sin120595 sin120593 sdot 119881 +

1198812sin2120595 tan120593119877


1198812 sin120595 cos120595 tan120593119877

minus1198812 sin120595 cos120595

119877+ 2120596119894119890cos120593 cos120595 sdot 119881 +

1198812sin2120595119877

]]]]]]]

]

(9)


120601119904119909= minus

1

119892(nabla119873+ nabla119889119899) (10)

120601119904119910=1

119892(nabla119864+ nabla119889119890) (11)

120601119904119911=120576119864+ 120576119889119890

120596119894119890cos120593

+1198704(1 + 119870

2) (120576119906+ 120576119889119906)

119877 sdot 1198703

(12)










119894119890 harmful



120596119901

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879 (13)

120596119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos120593]119879

(14)

119861119901

= (120596119901

119890119901+ 2120596119901

119894119890) times 119881119901

(15)



119881119901

dvl = 119862119901

119887119881119887

dvl (16)



fb

Cp

b

Cp

b

120596pc

120596bip

120596bib

120596pie

R

120596pep

120596bip = Cbp

120596bc = 120596pcCb

p

(120596pie + 120596pep)

120596bc

minus

minus

minus

minus

Cp

b

Cp

b

fp = Cp

bfb

120593

120579

120574

120595

Ap

Bp



Cp

b = Cp

btimes 120596bpb

Vp

dvl

Vp

dvlVb

dvlVp

dvl = Cp

bVb

dvl

120593 = 1205930 minus intVP

dvlN



120593 = 1205930minus int

119881119901

dvl119873119877

(17)

119881119901










119881119887

dvl = 119881119887

+ Δ119881119887

119888 (18)


119881119901

dvl = 119881119901

+ 119862119901

119887Δ119881119887

119888 (19)





119890119901and



(1) The Influence 120596119901119890119901


119890119901by



120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)


1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)


1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)




119881119889119890and 120576119881119889119906


1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)




1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)



119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)


119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)


120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)


119864and 120575119861119901



nabla119861119889119899


1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873


119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)


120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)


119894119890


and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










fb

fp = Cp

bfb

Cp

b

Cp

b

Cp

b

120596bc = Cp

b120596pc

120596pc

fp

120596bip

120596bib

120596pie

120596pie

120596pep

120596bip = Cbp( + 120596pep)

Attitude


120596bc minus

minus

Cp

b = Cp

btimes 120596bpb







119901

119887is

as follows

120596119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890)

119901

119887= 119862119901

119887times 120596119887

119901119887

(1)


119887





120596119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888) (2)











1198701= 3120590 119870

2= (2 +

1

1205852)1205902

1205962119904

minus 1 1198703=

1205903

12058521205962119904

120596119904= radic

119877

119892 120590 = 120585120596

119899

(3)






1198701= 1198703= 2120590 119870

2=1205902 + 1205962

119899

1205852 sdot 1205962119904

minus 1 1198704=

41205904

1205852 sdot 1205962119904

120596119904= radic

119877

119892




120601119904119909= minus

1

119892nabla119873 (5)

120601119904119910=1

119892nabla119864 (6)

120601119904119911=

120576119864

120596119894119890cos120593

+1198704(1 + 119870

2) 120576119906

119877 sdot 1198703

(7)




this status


and 120596119901119894119890 According to


119890119901and 120596

119901



119890119901and 120596119901



119890119901and 120596119901

119894119890can be got



K3

s

K2

R

1

R

1

s

K1

+

+ +

+

minus

minus120596pcx

120576nnablan

ΔAN


K(s)

K2

R

1

R

1

s

K1

+

minus

+

+minus

pcx

nnablan

pcz

ΔAN






119873= 119881sdot cos120595 and119881





119890119899

yV

x

z

O

Oi

R

VE

VN

120593

Rcos120593

pN



[

[

120596119909

120596119910

120596119911

]

]

= [

[

0120596119894119890cos120593

120596119894119890sin120593

]

]

+

[[[[[[

[

minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 sdottan120593119877

]]]]]]

]

(8)



becomes120596119899119894119887= 120596119899119894119890+120596119899119890119899+120596119899119899119887+120576119899120596119899



119894119890+




119889


Static base

120596nib = 120596nie + 120576n


120596nib = 120596nie + 120596nen + 120576

n = 120596nie + 120576nd + 120576

n


Angularmotion

Constantvelocity

Linearmotion

Uniform motion




120576119899

119889= [120576119889119890120576119889119899

120576119889119906]119879

= 120596119899

119890119899

= [minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 tan120593119877

]119879

nabla119899

119889= [

[

nabla119889119890

nabla119889119899

nabla119889119899

]

]

= (2120596119899

119894119890+ 120596119899

119890119899) times 119881119899

=

[[[[[[[

[

2120596119894119890sin120595 sin120593 sdot 119881 +

1198812sin2120595 tan120593119877


1198812 sin120595 cos120595 tan120593119877

minus1198812 sin120595 cos120595

119877+ 2120596119894119890cos120593 cos120595 sdot 119881 +

1198812sin2120595119877

]]]]]]]

]

(9)


120601119904119909= minus

1

119892(nabla119873+ nabla119889119899) (10)

120601119904119910=1

119892(nabla119864+ nabla119889119890) (11)

120601119904119911=120576119864+ 120576119889119890

120596119894119890cos120593

+1198704(1 + 119870

2) (120576119906+ 120576119889119906)

119877 sdot 1198703

(12)










119894119890 harmful



120596119901

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879 (13)

120596119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos120593]119879

(14)

119861119901

= (120596119901

119890119901+ 2120596119901

119894119890) times 119881119901

(15)



119881119901

dvl = 119862119901

119887119881119887

dvl (16)



fb

Cp

b

Cp

b

120596pc

120596bip

120596bib

120596pie

R

120596pep

120596bip = Cbp

120596bc = 120596pcCb

p

(120596pie + 120596pep)

120596bc

minus

minus

minus

minus

Cp

b

Cp

b

fp = Cp

bfb

120593

120579

120574

120595

Ap

Bp



Cp

b = Cp

btimes 120596bpb

Vp

dvl

Vp

dvlVb

dvlVp

dvl = Cp

bVb

dvl

120593 = 1205930 minus intVP

dvlN



120593 = 1205930minus int

119881119901

dvl119873119877

(17)

119881119901










119881119887

dvl = 119881119887

+ Δ119881119887

119888 (18)


119881119901

dvl = 119881119901

+ 119862119901

119887Δ119881119887

119888 (19)





119890119901and



(1) The Influence 120596119901119890119901


119890119901by



120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)


1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)


1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)




119881119889119890and 120576119881119889119906


1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)




1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)



119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)


119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)


120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)


119864and 120575119861119901



nabla119861119889119899


1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873


119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)


120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)


119894119890


and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










K3

s

K2

R

1

R

1

s

K1

+

+ +

+

minus

minus120596pcx

120576nnablan

ΔAN


K(s)

K2

R

1

R

1

s

K1

+

minus

+

+minus

pcx

nnablan

pcz

ΔAN






119873= 119881sdot cos120595 and119881





119890119899

yV

x

z

O

Oi

R

VE

VN

120593

Rcos120593

pN



[

[

120596119909

120596119910

120596119911

]

]

= [

[

0120596119894119890cos120593

120596119894119890sin120593

]

]

+

[[[[[[

[

minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 sdottan120593119877

]]]]]]

]

(8)



becomes120596119899119894119887= 120596119899119894119890+120596119899119890119899+120596119899119899119887+120576119899120596119899



119894119890+




119889


Static base

120596nib = 120596nie + 120576n


120596nib = 120596nie + 120596nen + 120576

n = 120596nie + 120576nd + 120576

n


Angularmotion

Constantvelocity

Linearmotion

Uniform motion




120576119899

119889= [120576119889119890120576119889119899

120576119889119906]119879

= 120596119899

119890119899

= [minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 tan120593119877

]119879

nabla119899

119889= [

[

nabla119889119890

nabla119889119899

nabla119889119899

]

]

= (2120596119899

119894119890+ 120596119899

119890119899) times 119881119899

=

[[[[[[[

[

2120596119894119890sin120595 sin120593 sdot 119881 +

1198812sin2120595 tan120593119877


1198812 sin120595 cos120595 tan120593119877

minus1198812 sin120595 cos120595

119877+ 2120596119894119890cos120593 cos120595 sdot 119881 +

1198812sin2120595119877

]]]]]]]

]

(9)


120601119904119909= minus

1

119892(nabla119873+ nabla119889119899) (10)

120601119904119910=1

119892(nabla119864+ nabla119889119890) (11)

120601119904119911=120576119864+ 120576119889119890

120596119894119890cos120593

+1198704(1 + 119870

2) (120576119906+ 120576119889119906)

119877 sdot 1198703

(12)










119894119890 harmful



120596119901

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879 (13)

120596119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos120593]119879

(14)

119861119901

= (120596119901

119890119901+ 2120596119901

119894119890) times 119881119901

(15)



119881119901

dvl = 119862119901

119887119881119887

dvl (16)



fb

Cp

b

Cp

b

120596pc

120596bip

120596bib

120596pie

R

120596pep

120596bip = Cbp

120596bc = 120596pcCb

p

(120596pie + 120596pep)

120596bc

minus

minus

minus

minus

Cp

b

Cp

b

fp = Cp

bfb

120593

120579

120574

120595

Ap

Bp



Cp

b = Cp

btimes 120596bpb

Vp

dvl

Vp

dvlVb

dvlVp

dvl = Cp

bVb

dvl

120593 = 1205930 minus intVP

dvlN



120593 = 1205930minus int

119881119901

dvl119873119877

(17)

119881119901










119881119887

dvl = 119881119887

+ Δ119881119887

119888 (18)


119881119901

dvl = 119881119901

+ 119862119901

119887Δ119881119887

119888 (19)





119890119901and



(1) The Influence 120596119901119890119901


119890119901by



120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)


1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)


1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)




119881119889119890and 120576119881119889119906


1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)




1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)



119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)


119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)


120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)


119864and 120575119861119901



nabla119861119889119899


1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873


119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)


120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)


119894119890


and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










Static base

120596nib = 120596nie + 120576n


120596nib = 120596nie + 120596nen + 120576

n = 120596nie + 120576nd + 120576

n


Angularmotion

Constantvelocity

Linearmotion

Uniform motion




120576119899

119889= [120576119889119890120576119889119899

120576119889119906]119879

= 120596119899

119890119899

= [minus119881 cos120595119877

119881 sin120595119877

119881 sin120595 tan120593119877

]119879

nabla119899

119889= [

[

nabla119889119890

nabla119889119899

nabla119889119899

]

]

= (2120596119899

119894119890+ 120596119899

119890119899) times 119881119899

=

[[[[[[[

[

2120596119894119890sin120595 sin120593 sdot 119881 +

1198812sin2120595 tan120593119877


1198812 sin120595 cos120595 tan120593119877

minus1198812 sin120595 cos120595

119877+ 2120596119894119890cos120593 cos120595 sdot 119881 +

1198812sin2120595119877

]]]]]]]

]

(9)


120601119904119909= minus

1

119892(nabla119873+ nabla119889119899) (10)

120601119904119910=1

119892(nabla119864+ nabla119889119890) (11)

120601119904119911=120576119864+ 120576119889119890

120596119894119890cos120593

+1198704(1 + 119870

2) (120576119906+ 120576119889119906)

119877 sdot 1198703

(12)










119894119890 harmful



120596119901

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879 (13)

120596119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos120593]119879

(14)

119861119901

= (120596119901

119890119901+ 2120596119901

119894119890) times 119881119901

(15)



119881119901

dvl = 119862119901

119887119881119887

dvl (16)



fb

Cp

b

Cp

b

120596pc

120596bip

120596bib

120596pie

R

120596pep

120596bip = Cbp

120596bc = 120596pcCb

p

(120596pie + 120596pep)

120596bc

minus

minus

minus

minus

Cp

b

Cp

b

fp = Cp

bfb

120593

120579

120574

120595

Ap

Bp



Cp

b = Cp

btimes 120596bpb

Vp

dvl

Vp

dvlVb

dvlVp

dvl = Cp

bVb

dvl

120593 = 1205930 minus intVP

dvlN



120593 = 1205930minus int

119881119901

dvl119873119877

(17)

119881119901










119881119887

dvl = 119881119887

+ Δ119881119887

119888 (18)


119881119901

dvl = 119881119901

+ 119862119901

119887Δ119881119887

119888 (19)





119890119901and



(1) The Influence 120596119901119890119901


119890119901by



120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)


1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)


1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)




119881119889119890and 120576119881119889119906


1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)




1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)



119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)


119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)


120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)


119864and 120575119861119901



nabla119861119889119899


1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873


119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)


120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)


119894119890


and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










fb

Cp

b

Cp

b

120596pc

120596bip

120596bib

120596pie

R

120596pep

120596bip = Cbp

120596bc = 120596pcCb

p

(120596pie + 120596pep)

120596bc

minus

minus

minus

minus

Cp

b

Cp

b

fp = Cp

bfb

120593

120579

120574

120595

Ap

Bp



Cp

b = Cp

btimes 120596bpb

Vp

dvl

Vp

dvlVb

dvlVp

dvl = Cp

bVb

dvl

120593 = 1205930 minus intVP

dvlN



120593 = 1205930minus int

119881119901

dvl119873119877

(17)

119881119901










119881119887

dvl = 119881119887

+ Δ119881119887

119888 (18)


119881119901

dvl = 119881119901

+ 119862119901

119887Δ119881119887

119888 (19)





119890119901and



(1) The Influence 120596119901119890119901


119890119901by



120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)


1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)


1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)




119881119889119890and 120576119881119889119906


1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)




1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)



119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)


119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)


120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)


119864and 120575119861119901



nabla119861119889119899


1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873


119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)


120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)


119894119890


and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










(1) The Influence 120596119901119890119901


119890119901by



120575120596119901v119890119901= [minus

120575119881119873

119877

120575119881119864

119877

120575119881119864

119877 cos120593]119879

(20)


1205961015840119887

119901119887= 120596119887

119894119887minus 119862119887

119901(120596119901

119890119901+ 120575120596119901

119890119901+ 120596119901

119894119890+ 120575120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887minus 119862119887

119901(120575120596119901V119890119901) minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(21)


1205961015840119887

119901119887= 120596119887

119894119887+ 120576119887minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

= 120596119887

119894119887+ 119862119887

119901120576119901minus 119862119887

119901(120596119901

119890119901+ 120596119901

119894119890) minus 119862119887

119901(120596119901

119888)

(22)




119881119889119890and 120576119881119889119906


1206011205751198811

119911=120576119881119889119890

120596119899119894119890

+119870119911(1 + 119870

2) 120576119881119889119906

119877 sdot 1198703

=1

120596119894119890cos120593

sdot (minus120575119881119873

119877) +

119870119911(1 + 119870

2)

1198703

sdot120575119881119864

1198772 cos120593

(23)




1206011205751198811

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877) (24)



119861119901

119864= (2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119873

119861119901

119873= minus(2120596

119894119890sin120593 + 119881

119864

119877 cos120593) sdot 119881119864

(25)


119873 (25) can

be converted as

1198611015840119901

119864= (2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119873+ 120575119881119873)

1198611015840119901

119873= minus(2120596

119894119890sin120593 + 119881119864 + 120575119881119864

119877 cos120593) sdot (119881119864+ 120575119881119864)

(26)


120575119861119901

119864= 2120596119894119890sin120593 sdot 120575119881

119873+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593

120575119861119901

119873= minus(2120596

119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(27)


119864and 120575119861119901



nabla119861119889119899


1206011205751198812

119909= minus

nabla119861119889119899

119892

= minus1

119892sdot ( minus 2120596

119894119890sin120593 sdot 120575119881

119873


119877 cos120593)

1206011205751198812

119910=nabla119861119889119890

119892=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864

+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

(28)


120601120575119881

119909=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119873

+120575119881119873sdot 119881119864+ 120575119881119864sdot 119881119873+ 120575119881119864sdot 120575119881119873

119877 cos120593)

120601120575119881

119910=1

119892sdot (2120596119894119890sin120593 sdot 120575119881

119864+2120575119881119864sdot 119881119864+ 1205751198812119864

119877 cos120593)

120601120575119881

119911=

1

120596119894119890cos120593

sdot (minus120575119881119873

119877)

(29)


119894119890


and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










and 120596119901119890119901



119890119901


and 120596119901119890119901



calculated by

1205961015840119901

119894119890= [0 120596

119894119890cos1205931015840 120596

119894119890sin1205931015840]119879 (30)



120575120596119901120593

119894119890=[[

[

0

120596119894119890(cos1205931015840 minus cos120593)

120596119894119890(sin1205931015840 minus sin120593)

]]

]

=

[[[[[

[

0

120596119894119890sdot (minus2 sin(120593 +

120575120593

2) sin 120575120593)

120596119894119890sdot (2 cos(120593 +

120575120593

2) sin 120575120593)

]]]]]

]

(31)


1205961015840119901

119890119901= [minus

119881119873

119877

119881119864

119877

119881119864

119877 cos1205931015840 ]119879

(32)


from (32)

120575120596119901120593

119890119901= [0 0

119881119864

119877 cos120593minus

119881119864

119877 cos1205931015840 ]119879

= [0 0119881119864

119877sdot [minus

2 sin (120593 + 1205751205932) sin 120575120593cos1205931015840 cos120593

]]

119879

(33)


and 120575120596119901120593119894119890


119890119901and

120575120596119901120593

119894119890

120575120596119901

119890119901+ 120575120596119901

119894119890=

[[[[[

[

0


120575120593

2))

120596119894119890sin 120575120593 sdot (2 cos(120593 +

120575120593

2)) minus

sin 120575120593119877

sdot [2119881119864sin (120593 + 1205751205932)cos1205931015840 cos120593

]

]]]]]

]

(34)


119894119890sin 120575120593 and sin 120575120593119877 in (34)



1 1198702 1198703 and 119870



1198701= 1198703= 2120585120596

119899

1198702=1198771205962119899(1 + 1205852)

119892minus 1

119870 (119904) =11987712058521205964119899

119892

(35)


119894119890and 119892 are



119901=


119901= (119860120596) cos120593 minus (119860120596) cos(120596119905 + 120593) For


119860119889119899and nabla


nabla119860119889= [

[

nabla119860119889119890

nabla119860119889119899

nabla119860119889119906

]

]

= (2120596119899

119894119890+ 120596119899

119875) times 119881119899

119875


=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










=

[[[[[[[[

[

119860119873+ 2120596119894119890sin120595 sin120593 sdot 119881

119875+1198812119901sin2120595 tan120593119877


119875minus1198812119901sin120595 cos120595 tan120593

119877minus1198812119901sin120595 cos120595119877

+ 2120596119894119890cos120593 cos120595 sdot 119881

119901+1198812119901sin2120595119877

]]]]]]]]

]

(36)


119860119889119899 120576119860119889119890

and 120576119860119889119906

120576119860119889= [120576119860119889119890

120576119860119889119899

120576119860119889119906]119879

= 120596119899

119901

= [minus119881119875cos120595119877

119881119875sin120595119877

119881119875sin120595 tan120593119877

]119879

(37)


120601119860

119909(119904) =

minus ((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119899

(119904)

120601119860

119910(119904) =

((1 + 21205852) 1205962119899119892) (119904 + 120585120596

119899 (1 + 21205852))

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)

sdot nabla119860119889119890

(119904)

120601119860

119911(119904) =

12058521205964119899Ω cos120593

(1199042 + 2120585120596119899119904 + 1205962119899) (119904 + 120585120596

119899)2sdot 120576119860119889119899

(119904)

(38)














119899

119887= 119862119899

119887Ω119899

119899119887 (39a)

V119899 = 119862119899119887119891119887

119894119887minus (2120596

119899

119894119890+ 120596119899

119890119899) times V119899 + 119892119899 (39b)

=V119899119873

119877 =

V119899119864sec120593119877

(39c)

Among them

Ω119887

119899119887= (120596119887

119899119887times) 120596

119887

119899119887= 120596119887

119894119887minus (119862119899

119887)119879

(120596119899

119894119890+ 120596119899

119890119899)

119892119899

= [0 0 minus119892]119879

(40a)

120596119899

119894119890= [0 120596

119894119890cos120593 120596

119894119890sin120593]119879

120596119899

119890119899= [minus

V119899119873

119877V119899119864

119877V119899119864tan120593119877

]

119879

(40b)












tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra














tk

tk1

tk2

tkn

tmt0

tk1 + ΔT

tk2 + ΔT

tkn + ΔT

tk + ΔT



119862119899

119887119896= 119862119899

119887119896minus1(119868 + Δ119879 sdot Ω

119887

119899119887119896) (41a)

V119899119896= V119899119896minus1

+ Δ119879 sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


]

(41b)

120593119896= 120593119896minus1

+Δ119879 sdot V119899

119873119896minus1

119877

120582119896= 120582119896minus1

+Δ119879 sdot V119899

119864119896minus1sec120593119896minus1

119877

(41c)

Among them

Ω119887

119899119887119896= (120596119887

119899119887119896times)

120596119887

119899119887119896= 120596119887

119894119887119896minus (119862119899

119887119896minus1)119879

(120596119899

119894119890119896minus1+ 120596119899

119890119899119896minus1+ 120596119899

119888119896minus1)

120596119899

119894119890119896= [0 120596

119894119890cos120593 120596

119894119890sin120593119896]119879

(42a)

120596119899

119890119899119896= [minus

V119899119873119896

119877V119899119864119896

119877V119899119864119896

tan120593119896

119877]

119879

(119896 = 1 2 3 )

(42b)



1198870= 119862119899119887119898 V1198990= minusV119899

119898 1205930= 120593119898 and



119898(point B)




119862119899

119887119896minus1= 119862119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896)minus1

asymp 119862119899

119887119896(119868 minus Δ119879 sdot Ω

119887

119899119887119896) asymp 119862

119899

119887119896(119868 + Δ119879 sdot Ω

119887

119899119887119896minus1)

(43a)

V119899119896minus1

= V119899119896minus Δ119879

sdot [119862119899

119887119896minus1119891119887

119894119887119896minus (2120596

119899

119894119890119896minus1+ 120596119899


+ 119892119899

]


119899

119887119896119891119887

119894119887119896minus1minus (2120596

119899

119894119890119896+ 120596119899

119890119899119896) times V119899119896+ 119892119899

]

(43b)

120593119896minus1

= 120593119896minusΔ119879 sdot V119899

119873119896minus1


119873119896

119877 (43c)

120582119896minus1

= 120582119896minusΔ119879 sdot V119899

119864119896minus1sec120593119896minus1


119864119896sec120593119896

119877

(43d)

Among them

Ω119887

119899119887119896minus1= (119887

119899119887119896minus1times)

119887

119899119887119896minus1= minus [120596

119887

119894119887119896minus1minus (119862119899

119887119896)119879

(120596119899

119894119890119896+ 120596119899

119890119899119896+ 120596119899

119888119896)]

(44)

5 Simulation









Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra















Time (min)0 10 20 30 40 50 60

minus005

0

005

01

Easte

rn m

isalig

nmen

tan

gle (

∘ )




1198961= 1198962= 00113

119896119864= 119896119873= 981 times 10

minus6

119896119880= 41 times 10

minus6

(45)





Time (min)

0 10 20 30 40 50 60minus005

0

005

01

Nor

ther

n m

isalig

nmen

tan

gle (

∘ )


Time (min)0 10 20 30 40 50 60

Hea

ding

misa

lignm

ent

angl

e (∘ )

3

4

2

1

0

minus1


0

minus50

minus100Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

0

minus50

minus100

minus150


Frequency (rads)

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus605

Y minus7253

Y minus1763

Y minus1725







minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










minus100

minus200

100

50

0

0

minus300


Mag

nitu

de (d

B)Ph

ase (

deg)

Bode diagram

Frequency (rads)

minus50

minus100

wn = 002

wn = 001

X 0314

X 0314

X 0314

X 0314

Y minus1368

Y minus3774

Y minus3542

Y minus3483


0 500 1000 1500 2000 2500 3000minus02

minus01

0

01

02

03

04

05

Misa

lignm

ent a

ngle

(deg

)

2050 2060 2070 2080

0

0005

001

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent

angl

e (de

g)

minus0015

minus001

minus0005






119899= 002 to 120596


1=

00240 1198702= 14769 and 119870


1198701= 1198703= 0016 119870

2= 1059534 and 119870

4= 00042 The


0

10

20

30

40

50

2140 2150 2160 2170

minus005

0

005

01

0 500 1000 1500 2000 2500 3000

Time (s)

Time (s)

wn = 002

wn = 001

Misa

lignm

ent a

ngle

(deg

)

Misa

lignm

ent

angl

e (de

g)

minus10




6 Test Verification





computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra











computer

UPS power

DVL

GPS


12014

12016

12018

1202

12022

12014

12016

12018

1202

12022

12024

120123115 312

3125 313 3135 314

3125 313 3135 314 3145 315

Trajectory


Finishing point

Long

itude





stability lt100 ppm


range plusmn15 g



3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

155

160

165

Hea

ding

(∘)

Roll

(∘)

Pitc

h(∘

)


minus10

0

10

5

10

15

Time (s)





minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










minus1

minus05

minus15

05

005

1

Pitc

h er

ror(

∘ )Ro

ll er

ror(∘)

1960 1970 1980 1990

minus02

0

02

04

Method 1Method 2

Method 1Method 2

Method 1Method 2

minus5

minus4

minus3

minus 2

2

minus1

1

0

02

0

minus02

1990 1995 2000

Hea

ding

erro

r(∘ )

minus10

minus5

0

5

10

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

0 200 400 600 800 1000 1200 1400 1600 1800 2000

t (s)









minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra













minus02

minus010

01

02

Time (s)3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

3800 4000 4200 4400 4600 4800 5000 5200 5400 5600 5800

024

6

8

Acce

lera

tion

(m)

Velo

city

(m)

Time (s2)

DVL velocity

DVL acceleration


0 01 02 03 04 050

01

02

03

04

05

06

07

08

09


Frequency (Hz)

Am

plitu

deF(j120596)





Method 2Method 3

Hea

ding

erro

r(∘ )

200 400 600 800 1000 1200 1400 1600 1800 2000

minus4

minus2

0

2

4

6

8

t (s)





corresponds to 120596119899= 0007)




0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










0 200 400 600

0

2

4

6

8

10

12

0

2

4

6

8

10

12

0

2

4

6

8

10

12

600 400 200 0 200 400 600

Hea

ding

erro

r(∘ )

t (s) t (s)t (s)






Mean 00646 00646 00645 00645Variance 01107 01109 01105 01105

Yaw error(∘)





7 Conclusion


Abbreviations





119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra










119862119901













120576119860119889119899

120576119860119889119906


nabla119860119889119899

nabla119860119889119890

nabla119860119889119906






Acknowledgments


References






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra



















Volume 2014




Journal of











Journal of


Function Spaces






Algebra
















Volume 2014




Journal of











Journal of


Function Spaces






Algebra









Documents

Research Article Error Analysis and Compensation of Gyrocompass Alignment for …downloads.hindawi.com/journals/mpe/2014/373575.pdf · 2019-07-31 · the gyrocompass alignment and