research article dual-model reverse ckf algorithm in

Research ArticleDual-Model Reverse CKF Algorithm in CooperativeNavigation for USV

Bo Xu Yong ping Xiao Wei Gao Yong gang Zhang Ya Long Liu and Yang Liu

Harbin Engineering University 145 Nantong Road Harbin 150001 China

Correspondence should be addressed to Bo Xu xubocartersinacom

Received 30 March 2014 Accepted 10 June 2014 Published 6 November 2014

Academic Editor Yan Liang

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

As one of the most promising research directions cooperative location with high precision and low-cost IMU is becoming anemerging research topic in many positioning fields Low-cost MEMSDVL is a preferred solution for dead-reckoning in multi-USV cooperative network However largemisalignment angles and large gyro drift coexist in low-costMEMS that leads to the poorobservability Based on cubature Kalman filter (CKF) algorithm that has access to high accuracy and relative small computationdual-model filtering scheme is proposed It divides the whole process into two subsections that cut off the coupling relationsand improve the observability of MEMS errors it first estimates large misalignment angle and then estimates the gyro driftFurthermore to improve the convergence speed of large misalignment angle estimated in the first subsection ldquotime reversionrdquoconcept is introduced It uses a short period time to forward and backward several times to improve convergence speed effectivelyFinally simulation analysis and experimental verification is conducted Simulation and experimental results show that the algorithmcan effectively improve the cooperative navigation performance

1 Introduction

In recent years cooperative navigation technology has beengradually expanded in satellite [1] multirobot [2ndash4] intelli-gent underwater vehicles [5] andmany other fields Researchteam named Stergios I Roumeliotis from America has con-ducted a series of theoretical studies for colocation of robotsincluding observability analysis estimation error analysisconsistency analysis and formation optimization [6] As oneof the most promising research directions in navigation andpositioning cooperative navigation is attracting more andmore attention in colocation industry and academia

Unmanned surface vehicle (USV) refers to running craftson surface of water that can be remotely operated by a personor operate independently It has characteristic of small sizehigh mobility and low-cost Multi-USV formation can takeadvantage of network using coordination and communica-tion technology The US Navy issued ldquothe navy unmannedsurface craft master planrdquo [7] that shows the significance ofcooperative combat techniques of multiple USV applied inthe military Cooperative navigation of multi-USV can be

commonly divided into two types according to configurationone is master-slave (leader-follower) structure (some withhigh-precision and others with low precision) the other isparallel structure (navigation accuracy of each USV is equiv-alent) Compared to master-slave structure parallel cooper-ative navigation cannot reconcile the contradiction betweenhigh-precision and low-cost fundamentally so master-slaveUSV cooperative navigation is the mainstream currently

With the rapid development of computer and drivenby demand of high-performance filtering in engineeringnonlinear filtering theory has been greatly developed [8ndash14] EKF UKF and PF are three commonly used nonlinearfiltering methods in practical applications EKF has highcalculating efficiency but the precision of EKF is limitedbecause of the Taylor expansion that neglects high-orderterms UKF and PF have good performance in some appli-cations however covariance calculated in the UKF filteringprocess might be nonpositive definite that leads to numericalinstability and filtering divergence while PF is easy to fall intothe particles degradation problem and heavy computationalcomplexity for high-dimensional applications [12] CKF can

Hindawi Publishing CorporationMathematical Problems in EngineeringVolume 2014 Article ID 186785 17 pageshttpdxdoiorg1011552014186785

2 Mathematical Problems in Engineering

avoid linearization of the nonlinear system by using cubaturepoint sets to approximate the mean and variance Becauseof its high accuracy and low calculation load CKF [14]has become a hotspot in research In this paper CKF isintroduced in the cooperative navigation with nonlinearmodel

As a branch of inertial field inertial systems based onmicroelectromechanical system (MEMS) gained rapid devel-opment in recent years Because of its low-cost small sizelight weight and high reliability there have been more andmore applications in low-cost inertial systems It has becomea preferred solution for the inertial navigation device onfollower boats in multi-USV cooperative network Howeverdue to the limitations of accuracy it is necessary to constructreasonable error model of MEMS based on which estimationand compensation can be preferred [15] Commonly usederror compensation methods include RBF neural networkanalysis ARMA model used in the timing model of gyrorandom error and piecewise interpolation compensation ofscale factor [16] However coexistence of larger initial mis-alignment and larger gyro drift is still unavoidable in practicaluse that leads to poor observability in cooperative navigationBased on different character of initial misalignment and gyrodrift estimation with subsection idea is proposed to cut offthe coupling relations

In addition how to obtain accurate estimation perfor-mance in short time is also a key technique Li et al proposeda new alignment method for AUV with backtracking frame-work The fine alignment runs with the recorded data duringthe process of coarse alignment which effectively improvesthe convergence speed and improve navigation accuracy[17 18] Xixiang Liu also proposed gyrocompass alignmentmethod based on reverse loop to solve compass alignmentfor SINS onmoving base and it greatly reduces the alignmenttime To improve the convergence speed of large initialmisalignment ldquotime reversionrdquo concept is introduced in thispaper As will be confirmed by simulation and experimentalresults the convergence speed and estimation accuracy areimproved greatly

The rest of this paper is organized as follows Section 2presents cooperative navigation model of single master USVand the process of CKF In Section 3 based on observabilityanalysis of cooperative navigation dual-model method andreverse filtering are proposed that improve the convergencespeed and improve the accuracy of cooperative location Sim-ulation of the proposed algorithm is provided in Section 4Aiming at validating the proposed algorithm further theprocess and the results of experiment in Tai Lake are givenin Section 5 Section 6 gives data analysis and discussionsIn Section 7 a summary is provided and future researchdirections are discussed

2 Cooperative Navigation Modeling andProblem Statement

21 Fundamental Theory of Cooperative Navigation ModelingMaster USV and slave USV are equipped with hydroacousticcommunication modem respectively at their bottoms to

achieve information dissemination and distance measure-ments between USVs The masters accomplish navigationusing their high-precision device (usually strap-down inertialnavigation system combined with GPS) while the slaves usetheir own equipped Doppler velocity log (DVL) and attitudeand heading reference system (AHRS) based on MEMS toproceed dead-reckoning

The principle of multi-USV cooperative navigation sys-tem is as follows Firstly master USV sends out fixed fre-quency acoustic pulse signal according to preset time intervalWith this signal slave USV can calculate the relative distanceaccording to underwater acoustic propagation delay Thenmaster USV sends out its position information by hydroa-coustic modem in the form of broadcasting After receivingthis information slave USV can conduct information fusionwith dead-reckoning position ofmasterUSV and the relativedistance and then correct its navigation result using filteringalgorithms Moreover route planning of USV can be carriedout to improve the observability and further improve theaccuracy of the cooperative navigation system

Positioning errors will accumulate over time (as shownby the purple dotted ellipse shown in Figure 1) Therefore itcannot provide accurate positioning information for a longtime When the information sent by master USV is receivedthe slave USV can use cooperative navigation algorithm tocorrect the result of dead-reckoning and effectively reduce thepositioning error (as shown by the green dotted ellipse shownin Figure 1) Through this cyclical filtering and correctionslave USV with low-precision navigation device can realizeprecise location for a long time [19 20]

22 System Model of Single Master USV When the followerdoes not receive the information sent by leaders it can onlyuse its own DVL and low accuracy MEMS to conduct dead-reckoning and themotionmodel can be expressed as follows

119909119878

119896+1= 119909119878

119896+ V119896sdot Δ119905 sdot sin (120572

119896)

119910119878

119896+1= 119910119878

119896+ V119896sdot Δ119905 sdot cos (120572

119896)

(1)

where 119909119878119896 119910119878119896represent the position of follower USV at time

119896 V119896represents the speed Δ119905means the time interval and 120572

119896

means the heading angle at time 119896 However the input modelof system in practice is as follows

V119896= (1 + 120575C

119896) (V119896+ 119908V119896)

120572119896= 119896minus 120575120572119896+ 119908120572119896

(2)

wherein V119896is the velocity measurements and 120575C

119896means the

scale factor error 119896 is the heading anglemeasured byMEMS120575120572119896= Δ120572119896+119896120576Δ119905 is error of

119896Δ120572119896is the initialmisalignment

Mathematical Problems in Engineering 3

tk

tkminus1tk+1

Slave USV

Trajectory

Parallel move

Master USV

Trajectory

Slave USV

Slave USVMaster USV

Master USV

Circle 1

Circle 2

Circle 3

Figure 1 Principle of cooperative navigation system

angle and 120576 is gyro drift Putting (2) into (1) the actualequation of states is as follows

119909119878

119896+1= 119909119878

119896+ (1 + 120575C

119896) (V119896+ 119908V119896) sdot Δ119905

sdot sin (119896minus Δ120572119896minus 120576119896sdot 119896 sdot Δ119905 + 119908

120572119896)

119910119878

119896+1= 119910119878

119896+ (1 + 120575C119896) (V119896 + 119908V119896) sdot Δ119905

sdot cos (119896 minus Δ120572119896 minus 120576119896 sdot 119896 sdot Δ119905 + 119908120572119896)

120575119862119896+1 = 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(3)

Motion model can be simplified asX119896+1= X119896+ 119879 (u119896w119896) (4)

in which X119896= (119909119896 119910119896 Δ120572119896 120576119896 120575119862119896)119879 means the state vector

of USV at time 119896 u119896= [V119896 119896]

119879 119879(u119896w119896) stands for the

nonlinear terms the process noise isw119896= [119908V119896 119908120572119896]

119879119908V119896 sim

119873(0 1205902

V119896) and 119908120572119896 sim 119873(0 1205902

120572119896) The noise covariance matrix

is

Q119896= 119864 w

119896w119879119896 = [

1205902

V1198961205902

120572119896

] (5)

Systematic observation equation can be expressed as

119884119896= 119877119896+ 119881119896= radic(119909

119872

119896minus 119909119878

119896)

2+ (119910119872

119896minus 119910119878

119896)

2+ 119881119896 (6)

wherein 119884119896stands for the measurement of distance 119877

119896

between follower and leader (119909119872119896 119910119872

119896) represents the posi-

tion of leaderUSVat time 119896while (119909119878119896 119910119878


tion of follower and119881119896 sim 119873(0 1205902

119903) is the measurement noise

23 Cubature Kalman Filter (CKF) Cubature Kalman filter(CKF) is proposed based on the spherical-radial cubature cri-terion [14] The core of the method is that the mean andvariance of probability distribution can be approximated bycubature points CKF first approximates the mean and var-iance of probability distribution through a set of 2119873 (119873is the dimension of the input random vector) cubature

points with the same weight propagates the above cubaturepoints through nonlinear functions and then calculates themean and variance of the current approximate Gaussian dis-tribution by the propagated cubature points

A set of cubature points are given by [120585119894119882119894] [14] where

120585119894is the 119894th cubature point and119882

119894is the correspondingweight

120585119894= radic

119898

2

[l]119894

119882119894 =

1

119898

(7)

wherein 119894 = 1 2 119898 119898 = 2119873Systematic state equation and observation equation are

expressed in (4) and (6) Assuming the posterior density attime 119896 minus 1 is known the steps of CKF are shown as follows[14]

Time update is as follows

119875119896minus1|119896minus1

= 119878119896minus1|119896minus1

119878119879

119896minus1|119896minus1 (8a)

119883119894 119896minus1|119896minus1

= 119878119896minus1|119896minus1

120585119894+ 119883119896minus1|119896minus1

(8b)

119883lowast

119894 119896|119896minus1= 119891 (119883

119894 119896minus1|119896minus1) (8c)

119883119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119883lowast

119894 119896|119896minus1 (8d)

119875119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119883lowast

119894 119896|119896minus1119883lowast119879

119894 119896|119896minus1minus 119883119896|119896minus1

119883119879

119896|119896minus1+ 119876119896minus1

(8e)

Measurement update is as follows

119875119896|119896minus1

= 119878119896|119896minus1

119878119879

119896|119896minus1 (9a)

119883119894 119896|119896minus1

= 119878119896|119896minus1

120585119894+ 119883119896|119896minus1

(9b)

119885119894 119896|119896minus1 = ℎ (119883119894 119896|119896minus1) (9c)

119885119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

(9d)


119875119911119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

119885119879

119894 119896|119896minus1minus119885119896|119896minus1

119885119879

119896|119896minus1+ 1205902

119903 (9e)

119875119909119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119883119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1 (9f)

With the new measurement vector 119884119896 the estimation of

the state vector 119883119896|119896

and its covariance matrix 119875119896|119896

at time 119896can be achieved by the following equations

119870119896= 119875119909119911

119896|119896minus1(119875119911119911

119896|119896minus1)

minus1

(10a)

119883119896|119896= 119883119896|119896minus1

+ 119870119896(119884119896minus119885119896|119896minus1

) (10b)

119875119896|119896= 119875119896|119896minus1

minus 119870119896119875119911119911

119896|119896minus1119870119879

119896 (10c)

CKF uses cubature rule and 2119873 cubature point sets[120585119894119882119894] to compute the mean and variance of probability

distribution without any linearization of a nonlinear modelThus the model can reach the third-order or higher

24 Problem Statement-Poor Observability of Error in MEMSA global observability analysis method proposed in [20]indicates that whether a state can be observed or not meansif there exists unique solution of the equation The problemthat whether the initial misalignment error and MEMS gyrodrift has unique solution will be preliminarily discussed inthis sectionHere in order to facilitate analysis only the initialmisalignment and gyro drift in MEMS are considered in thesystem temporarily Equation (2) can be expressed as follows

119877119896+1 = ((119909119878

119896+ V119896 sdot Δ119905 sdot sin (119896 minus 120575120572119896 + 119908120572119896) minus 119909

119872

119896+1)

2

+ (119910119878

119896+ V119896sdot Δ119905 sdot cos (

119896minus 120575120572119896+ 119908120572119896) minus 119910119872

119896+1)

2

)

12

(11)

from which we can obtain the expression of 120575120572119896

120575120572119896= arcsin

(119877119896+1)2minus (119909119878

119896minus 119909119872

119896+1)

2

minus (119910119878

119896minus 119910119872

119896+1)

2

minus V2119896Δ1199052

2V119896Δ119905radic(119909

119878

119896minus 119909119872

119896+1)

2

+ (119910119878

119896minus 119910119872

119896+1)

2

minus 120573 minus 119896

(12)

where 120573 = arctan((119910119878119896minus 119910119872

119896+1)(119909119878

119896minus 119909119872

119896+1)) So 120575120572

119896can

be calculated that means it is observable However theproportion of initial misalignment and MEMS gyro driftcannot be accurately known Therefore the observability ispoor

As mentioned above poor observability of MEMS isthe main issue in cooperative navigation The elaborateobservability analysis and corresponding solutions will beprovided in Section 3 below

3 Cooperative Navigation Modeling andObservability Analysis

31 Observability Analysis Observability directly determinesthe colocation accuracy of cooperative navigation system[20] the observability theory of linear systems will beno longer suitable to multi-AUV cooperative navigationwith nonlinear system The idea here is to linearize thenonlinear model firstly and then to use observability the-ory of linearized systems for observability analysis Thesystem in this paper is stochastic system However theobservability analysis method of stochastic system can beequivalent to the corresponding deterministic system when119876 gt 0 and 119877 gt 0 [21 22] which is satisfied in ourpaper

Equation (1) can be abbreviated as X119896+1 = 119891(X119896 u119896) =X119896 + 119879(u119896w119896) Y119896 = ℎ[sdot] wherein X119896 means the state ofUSV at time 119896 and 119879(u119896w119896) stands for the nonlinear termsAssume X

119896= [119909119878

119896119910119878

119896Δ120572119896120576119896120575119862119896]119879 and 120575120572

119896= Δ120572119896+ 120576 sdot 119896 sdot

Δ119905 then state equations can be written as

119909119878

119896+1=119909119878

119896+ (1 + 120575C119896) (V119896 + 119908V119896) sdot Δ119905 sdot sin (119896 minus 120575120572119896 + 119908120572119896)

119910119878

119896+1=119910119878

119896+ (1 + 120575C

119896) (V119896+ 119908V119896) sdot Δ119905 sdot cos (119896 minus 120575120572119896 + 119908120572119896)

120575119862119896+1= 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(13)

Therefore the Jacobi matrix is

F119896=

120597119891

120597X119896

=

[

[

[

[

[

[

[

[

1 0 minusV119896Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896) minusV119896119896Δ1199052 cos (119896 minus 120575120572119896 + 119908120572119896) (V119896 + 119908V119896) Δ119905 sin (119896 minus 120575120572119896 + 119908120572119896)

0 1 V119896Δ119905 sin (

119896minus 120575120572119896+ 119908120572119896) V119896119896Δ1199052 sin (

119896minus 120575120572119896+ 119908120572119896) (V

119896+ 119908V119896) Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896)

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1

]

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X119896

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0 0] = [

Δ119909119896

119877119896

Δ119910119896

119877119896

0 0 0] = [sin 119896 cos 1198960 0 0]

(14)

where Δ119909119896= 119909119878

119896minus 119909119872

119896 Δ119910119896= 119910119878

119896minus 119910119872

119896


According to rank criterion observability matrix is

Γ =

[

[

[

[

[

[

H119896

H119896minus1F119896minus1H119896minus2F119896minus2H119896minus3F119896minus3H119896minus4

F119896minus4

]

]

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0 0

sin 119896minus1 cos 119896minus1 minusV119896minus1Δ119905 sin120572

1015840

119896minus1minusV119896minus1 (119896 minus 1) Δ119905

2 sin1205721015840119896minus1

(V119896minus1 + 119908V119896minus1) Δ119905 cos1205721015840

119896minus1



2 sin1205721015840119896minus2


119896minus2



2 sin1205721015840119896minus3


119896minus3



2 sin1205721015840119896minus4


119896minus4

]

]

]

]

]

]

]

]

]

]

]

]

]

]

]

(15)

10 20 30 40 50 60 70 80 90 100 110 1200

01

02

03

04

05

06

07

08

09

1

100 102 104 106 108 110 112 114 116 118 1200008

00085

0009

00095

001

00105

0011

00115

0012

k

y

y1 = 1 (k minus 1)

y2 = 1 (k minus 2)y3 = 1 (k minus 3)y4 = 1 (k minus 4)

Figure 2 Relative analysis of the column in observability matrix

where 1205721015840119895= (120575120572119895 minus 120596120572119895

) (119895 = 119896 minus 1 119896 minus 2 119896 minus 3 sdot sdot sdot ) Thelinear relativity of 3rd column and 4th column in Γ would bediscussed here It can be seen that (Γ(23) sdotΔ119905)Γ(24) = 1(119896minus1)(Γ(33) sdot Δ119905)Γ(34) = (1(119896minus2)) ((Γ(43) sdot Δ119905)Γ(44)) = 1(119896minus3)(Γ(53) sdot Δ119905)Γ(54) = 1(119896 minus 4) and Γ(119894119895) stands for the elementin row 119894 column 119895 of the system observation matrix Figure 2shows the variation of corresponding value over time

As can be seen from Figure 2 when time 119896 gt 1001(119896minus1) 1(119896minus2) 1(119896minus3) 1(119896minus4) is approximately equalThat means the third and fourth columns of the observationmatrix Γ become linear-relative and the linear correlation

becomes stronger over time It can be concluded that theobservability is poor and it gets worse as time flies

32 Resolve Route 1 Dual-Model Filtering Algorithm Both ofinitial misalignment and gyro drift can cause the increaseof error angle The error coupled together results in poorobservability Error caused by the initial misalignment angleis a constant value while the error caused by gyro driftgradually increases over time Their properties are shown inFigure 3

The proportion of initial misalignment angle and gyrodrift in total error angle over time are shown in Table 1 andFigure 4 (assuming initial misalignment is 90∘ and gyro driftis 10∘h)

From the above analysis it can be seen that with the pass-ing of time gyro drift will take increasingly large proportionwhile the initial misalignment of MEMS occupies absolutelylarge part of total deviation angle in the beginningThereforewe can cut off the entire time into two phases In the firstphase initial misalignment angle is estimated and modifiedalone while the gyro drift is estimated in the second stage Aslong as length of the first phase is short enough it will notaffect the estimation accuracy that is

Subsection 1 X1198961= [119909119878

119896119910119878

119896Δ120572119896120575119862119896]

119879

Subsection 2 X1198962= [119909119878

119896119910119878

119896120576119896120575119862119896]

119879

(16)

Herewewill take a look at observability of the system aftersubsection When the system state vector is selected as X

1198961=

[119909119878

119896119910119878

119896Δ120572119896 120575119862119896

]

119879

the Jacobi matrix is

F119896=

120597119891

1205971198831198961

=

[

[

[

[

[

[

[

1 0 minusV119896Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896) (V119896 + 119908V119896) Δ119905 sin (119896 minus 120575120572119896 + 119908120572119896)

0 1 V119896Δ119905 sin (119896 minus 120575120572119896 + 119908120572119896) (V119896 + 119908V119896) Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896)

0 0 1 0

0 0 0 1

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X1198961

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0] = [sin 119896 cos 1198960 0]

(17)


According to the rank criterion observability matrix canbe written as follows

Γ =

[

[

[

[

H119896

H119896minus1

F119896minus1

H119896minus2

F119896minus2

H119896minus3

F119896minus3

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0

sin 119896minus1

cos 119896minus1

minusV119896minus1Δ119905 sin (120575120572

119896minus1minus 119908120572119896minus1) (V119896minus1+ 119908V119896minus1) Δ119905 cos (120575120572119896minus1 minus 119908120572119896minus1)

sin 119896minus2

cos 119896minus2



sin 119896minus3

cos 119896minus3



]

]

]

]

]

]

]

]

]

]

(18)

Table 1 The proportion varies in the first 500 s

Time (s) Proportion of initialmisalignment

Proportion ofgyro drift

100 09970 00030200 09939 00061300 09909 00091400 09878 00122500 09848 00152

where rank Γ = 4 that is to say the matrix Γ is fullrank so the system is observable For the second phaseof cooperative navigation system we can get the sameconclusion Therefore it can be concluded that the systembecomes observable whenever in the first phase or the secondphase after subsection

33 Resolve Route 2 Reverse Algorithm In Section 32 itshows that after using the segmented dual-model filteringstate observability can be significantly improved Howeverthe proportion of gyro drift in total deviation angle graduallyincreases as time flies If the time of first stage is too longthe gyro drift will seriously affect the accuracy of model andthe precision of initial misalignment estimation CurrentlyMEMS devices are with low precision that large initialmisalignment and large gyro drift coexist Therefore how toestimate the large initial misalignment angle in short time isanother problem we have to face

Sampling data in the cooperative navigation system istypically a time sequenceThe filter works with chronologicalsequence of real-time processing The real-time results canbe obtained without data storage However similar to strap-down inertial navigation system [18 23 24] one significantfeature of cooperative navigation system is that all the infor-mation used in filtering process can be completely storedThatmeans the data can be copied intomany identical copiesand each copy can be processed by different methods withoutinterference between them This feature is also called ldquothediverse existence of mathematical platformrdquo

It is easily conceivable that if storage capacity of thenavigation computer is large enough and computing poweris strong enough the data can be stored and analyzed manytimes in very short time By repeating the analysis in forward

and backward it is possible to improve the precision orshorten the time cost

Based on this idea we can take a short period of time asthe first stage and carry forward filtering at the first timewhilethe data is saved Then through repeated use of the data forbackward and forwardfiltering the large initialmisalignmentangle can be accurately estimated in short timeThe dream ofimproving accuracy of the initial misalignment angle in shorttime can be achieved The process is shown in Figure 5

The corresponding forward and reverse state equations ofthe algorithm are as follows In order to distinguish betweenthem we use ldquorarr rdquo and ldquolarrrdquo to present forward and reverserespectively

(1) Forward filtering is as follows

119878

119896+1=119878

119896+ (1 + 120575C

119896) (V119896+ 119908V119896) sdot Δ119905 sdot sin (119896 minus Δ120572119896 + 119908120572119896)

119910119878

119896+1= 119910119878

119896+ (1 + 120575C

119896) (V119896+ 119908V119896) sdot Δ119905 sdot cos (119896 minus Δ120572119896 + 119908120572119896)

120575C119896+1= 120575C119896

Δ120572119896+1 = Δ120572119896

(19)

(2) Backward filtering is as follows

119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1= Δ120572119896

(20)

The whole ldquosubsection + reverserdquo filtering process issummarized as follows (as shown in Figure 6) (1) firstinitialize the filter parameters (2) in the first subsection(a short period of time) conduct the initial misalignmentestimation ignoring effects of gyro drift and store the data atthe same time (3) use the stored data to estimate the initialmisalignment reversely (4) return to steps (2) and (3) andrepeat two or three times until the covariance of the statedecreases in demand range (the selection of repeated timeis discussed in Section 6) (5) after compensation of initialmisalignment using estimation result in the first subsectionestimate the gyro drift and conduct cooperative navigationfiltering in the second subsection


k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)

Figure 3 Characteristics of initial misalignment angle and gyro drift

t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n

Proportion of initial error angle

0 5000

01

02

03

04

05

06

07

08

09

1

t (s)

Proportion of gyro drift

Figure 4 The proportion of initial misalignment angle and gyro drift

2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1

Figure 5 Schematic diagram of reverse algorithm

4 Simulation Analysis

To verify effectiveness of the proposed algorithms and valid-ity of observability analysis above simulation is conducted inthis section Simulation environment settings are as follows

(1) A and C are leaders and B is follower Three USVsare all equipped with underwater acoustic equipmentfor communication and ranging A and C send theirposition alternately time interval 119879 is set to 5 s

(2) The initial positions of A B and C are (0200)(00) and (2000) A and C are equipped with GPSreceiver that can accomplish self-positioning and thepositioning error does not accumulate The mean ofpositioning error is 0 and variance is set to 2m

(3) USV B can only conduct dead-reckoning navigationwhen the information of A or C is not receivedThe speed and heading angle are given by DVL andMEMS respectively the initial misalignment is set to90∘ gyro drift is 10∘h

(4) 120575C119896of speedmeasured byDVL inUSVB is 0005 and

the noise is set to zero mean and variance is 05ms


Initialize ObservationObservationm = m minus Δ120572

and reinitialize

Subsection 1 Subsection 2

Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896

) middot Δt middot sin(k minus Δ120572k + w120572119896)

rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896

) middot Δt middot cos(k minus Δ120572k + w120572119896)

120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS

k + (1 + 120575Ck)( k + w119896) middot Δt middot sin(k minus Δ120572k + w120572119896

)larryS

k+1 =larryS

k + (1 + 120575Ck)( k + w119896) middot Δt middot cos(k minus Δ120572k + w120572119896

)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))

xSk+1 = xSk + (1 + 120575Ck)( k + w119896) middot Δt middot sin(k minus 120576k middot k middot Δt + w120572119896

)

ySk+1 = ySk + (1 + 120575Ck)( k + w119896) middot Δt middot cos(k minus 120576k middot k middot Δt + w120572119896

)

z1 z2 zm zm+1 zm+2

Figure 6 ldquoSubsection + reverserdquo filtering timing diagram

0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800

Follower USV (B)Leader USV (A)Leader USV (C)

X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)

Figure 7 Trajectories of USVs

(5) Noise of ranging measurement is set with zero meanand variance is set to 2m

(6) A B and C are in linear motion with speedupand speed-down The speed changes between 8msand 15ms by plusmn005ms2 without considering thedynamics and current disturbance

0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100

Initial error angle in subsection 1

subsection subsection

Estim

atio

n

Gyro drift in subsection 2

Gyro drift without subsectionInitial error angle without subsection

t (s)

Figure 8 Estimation comparison with and without subsection

The filter is initialized as follows 1198750

=

diag (10m)2 (10m)2 (001)2 (80∘)2 (20∘h)2 The tra-jecto ries of three USVs are shown in Figure 7

Results comparison of corresponding methods is givenin Section 41 It includes comparison between methods withsegmentation and without segmentation and also compar-ison of method without reverse filtering and with reversefiltering (reverse once and reverse twice)


200 400 600 800 1000 1200 140089

895

90

905

91

915

92

With subsectionWithout subsection

Initi

al m

isalig

nmen

t (∘ )

t (s)

(a) Initial alignment estimation (enlarge view)

0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)

(b) Gyro drift estimation in long time without subsection

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

SubsectionNo subsection

Time (s)

Posit

ion

erro

r (m

)

(c) Colocation error comparison

Figure 9 Comparison of filtering performance with and without subsection

41 Comparison of Filtering Performance with and withoutSubsection Thewhole simulation time is set as 6000 secondsand we take 1500 seconds for the first stage Accordingto ldquodual-model algorithm with subsectionrdquo proposed inSection 4 the curves of estimation are shown in Figures 8 and9

From the comparison in Figures 8 and 9 the followingconclusions can be drawn

(1) As can be seen from Figure 8 the initial misalign-ment angle without segmentation begins to convergewithin a certain range but diverges later (as previouslyanalyzed observability grows gradually worse overtime) The error estimation accuracy is better than

that without segment and themodification at the endof first stage effectively suppresses initial misalign-ment angle divergence

(2) It can be seen in Figures 8 and 9(b) that the estimationperformance of gyro drift without segmentation ispoor It gradually converges in a very long time whileit converges quickly after subsection

(3) As can be seen in Figure 9(a) the estimation of theinitial misalignment angle in subsection 1 is betterthan that without subsection but there is still atendency of divergence

(4) Figure 9(c) tells us that due to better estimation ofthe initial misalignment angle and gyro drift after

10 Mathematical Problems in EngineeringVe

loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5

(a) Velocity with reverse once

Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5

(b) Velocity with reverse twice

Figure 10 Velocity of follower USV

segmented the performance of colocation is betterthan nonsegments However such divergent trendstill exists in colocation estimation error due tocertain bias of initial heading angle estimation

In summary the performance with segment is betterthan that without segmentation But there is still space forfurther improvement especially in how to improve estimationaccuracy of the initial misalignment angle of the first phase

42 ldquoSubsection + Reverserdquo Algorithm The reverse filteringis introduced on the base of subsection in this section Itgives comparison of methods without reverse reverse onceand reverse twice The velocity curve of USV varies between8ms and 15ms without reverse It moves in a straight linewith acceleration and deceleration Figure 10 shows velocitywith reverse once and with reverse twice The correspondingperformance comparison is shown in Figure 11

To make the description more clearly several axes withdifferent colors are used in the following figures In Figures10 15 and 16 the blue time-axis stands for the algorithmwithreverse once andwith reverse twice And in Figures 11 17 and18 the time-axes with blue color green color and red colorrepresent the time axis of the algorithm without reverse withreverse once and with reverse twice respectively

43 Analysis of Simulation Results From Figure 11 it canbe seen that performance of ldquosubsection + reverserdquo filteringalgorithm is significantly better than that without reverseSpecific analyses are as follows

(1) From Figures 11(a) and 11(b) it can be seen that whenreverse filtering is not used the initial error convergesto a certain angle and diverges later with time At theend of the first subsection (1500 seconds) estimationreaches 92∘ When using reverse filtering once the

length of sampling data segment is shortened to500 seconds that reduces the impact of gyro driftThe precision of initial misalignment angle achievessignificant improvement It reaches 905∘ at the endof the first phaseWhen reverse filtering is used twicelength of sample data is shortened to 300 seconds thatreduce the impact of gyro drift further Estimationreaches 9005∘ in 300 seconds (which is the endof the first stage) Therefore estimation accuracy issignificantly improved it effectively suppresses thedivergence of initial misalignment angle

(2) As is shown in Figure 11(c) without the introductionof reverse filtering the estimated effect of the gyrodrift is poor It costs about 6000 seconds to convergeWith introduction of reverse filtering once gyro driftestimation converges in 3500 seconds while the errorestimation of gyro drift converges in 1500 secondsafter using reverse algorithm twice The convergencespeed becomes significantly faster

(3) It can be seen from Figure 11(c) that at the end ofthe first stage the initial misalignment is correctedBecause of the introduction of reverse filtering themodel becomes more accurate and the estimationaccuracy becomes much higher Therefore the preci-sion of cooperative navigation is always within 20mas shown in Figure 11(d)

5 Water Test Verification

To verify the feasibility of proposed cooperative navigationalgorithms further water experiment is conducted at Tai Lakein Wuxi city of Jiangsu Province For ease of operation oneself-made USV is used for followers and two patrol crafts arechosen to play the role of leader USVs instead


Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90

Without reverseReverse onceReverse twice

100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800

(a) Estimation comparison in whole process

Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)

(b) The partial enlarged view of initial misalignment

Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse

Reverse onceReverse twice

1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)

(c) An enlarged view of the gyro drift estimation

Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)

(d) Cooperative navigation error estimation

Figure 11 Comparison with no reverse reverse once and reverse twice

51 Overview of the Test The equipment installation is as fol-low As shown in Figure 12 each leader USV is equipped withGPS acoustic communication devices and data collectionsystem GPS is used for providing accurate positioninginformation and acoustic communications equipment (Tele-dyne Benthosrsquos ATM-885 sonar is selected that can achieve360∘ sonar signal transmission and reception) is used to

complete information transmission and measurement ofdistance between the USVs Follower USV is equipped withPHINS GPS DVLMEMS and also ATM-885 sonar PHINSworks in combination mode with GPS It is benchmark thatprovides high-accurate heading attitude and position andspeed information Dead-reckoning is conducted by speed(provided by DVL) and heading (provided by MEMS)


GPS Data collection system

PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)

Figure 12 Unmanned craft as follower (a) and two patrol crafts as leaders (b)

Acoustic sender 1

Acoustic sender 2

Data collecting system 1

GPS 2GPS 1

Position of A

Leader USV (A) Leader USV (B)

MEMS


PHINS

Navigation information


GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2

Position of A Position of B

Position of B

Figure 13 The equipment installation in follower (USV C)

The performance of each equipment is as shown inTable 2

Equipment installation and signal flow are as shown inFigure 13

The test procedure is as follows (1) two water patrolboats are selected as pilot USV (labeled as A and B) anda USV as follower (labeled as C) the computer clock issynchronized precisely (2) Dead-reckoning is conductedusing speed provided by DVL and heading angle providedby MEMS to obtain the real-time location (3) The twoleaders send their location information to C alternately usingacoustic device the interval is selected as 10 s (4) Afterreceiving aided information and distance information fusion

is made by using the dead-reckoning information locationinformation from leaders and the distance measurement

In the experiment complete test data are stored to makeoffline analysis and comparison of different filteringmethodsThe position information provided by GPS installed on USVC is used as benchmark to make comparison of differentmethods Similarly PHINS on C is used as the benchmarkfor heading Shot by Google earth map photo three USVstrajectories are shown in Figure 14 (about 20 kilometers of thewhole voyage)

52 Data Processing We take 1250 seconds for the first stagein which initial misalignment is estimated and gyro drift


Table 2 The performance of equipment

MEMS

Gyroscope bias stability lt30∘hGyroscope scale factor error 01Gyroscope measuring range plusmn300∘sAccelerometers bias stability 02mgAccelerometers threshold plusmn5 times 10

minus4 gAccelerometers measuring range plusmn10 g

PHINSPosition accuracy (CEP50) 05ndash3mHeading accuracy (1120590 value) 001∘ secant latitudeAttitude accuracy (1120590 value) Less than 001∘

GPSTime accuracy 1 usVelocity accuracy 01msPosition accuracy lt25m

DVL Working range minus150msndash200msMeasurement accuracy 01

Acoustic device Working range Up to 8000mError rate (with correction algorithm) Less than 10minus7

Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2

Figure 14 Trajectory of three vehicles

estimation is conducted 1250 seconds later Correspond-ing curves of three methods (including algorithm withoutreverse reverse once and reverse twice) are given in Figures15 to 18

53 Analysis From the simulation results in Figures 17 and18 discussion is made as follows

(1) Figure 17(a) shows that when backward filtering isnot used the initial misalignment converges to cer-tain angle but becomes divergence later Estimationreaches 95∘ at the 1250th second (which is the end ofthe first phase) When reverse filtering is used oncesampling data length is shortened to 417 seconds andthe impact of gyro drift is suppressed sharply Accu-racy of initial misalignment estimation is improved(that is 92∘ at 417th second) To reduce the impact ofgyro drift further reverse filtering is used twice Sam-pling data length is shortened to 250 seconds while

estimation becomes 899∘ in 250 seconds Accuracy ofestimation is significantly improved and the error iscorrected in the end of the first subsection

(2) From Figure 17(b) it can be seen that estimationperformance of gyro drift is poor without the intro-duction of reverse filtering It gradually converges in3500 seconds By using the reverse filtering once theconvergence of gyro drift costs about 1500 secondswhile it converges in 500 seconds by using the reversefiltering twice The convergence speed become muchfaster

(3) Figure 18(b) shows the divergent trend of cooperativenavigation error in 6000 seconds without inversealgorithm and the maximum error is larger than 500meters After the introduction of reverse filteringcooperative navigation and positioning accuracy issignificantly improved as initial misalignment angleerror and gyro drift are estimated more accuratelyThe maximum positioning error is less than 400meters with reverse once while it is within 300 meterswith reverse twice

6 Discussion

Through the above analysis and comparison of experimentaldata it can be concluded as follows

(1) Because dead-reckoning cannot provide precise posi-tioning information in long period the cooperativenavigation algorithm with CKF is introduced andldquosubsection + reverserdquo solution is also proposedto improve accuracy effectively while time cost isreduced

(2) There are still some problems to be solved in practi-cal applications For example ranging accuracy andthe communication quality greatly affect the perfor-mance of colocation In speedup a lot of bubbles


0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)

(a) The speed of follower (reverse once)

0

200

400

600

800

1000

1200

1400

Tim

e (s)

Latitude ( ∘) Longitude (∘ )

313631359

3135831357

31356 120298120302

12030612031

(b) Trajectory of follower USV (reverse once)

Figure 15 The trajectory and speed of follower (reverse once)

0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)

(a) Velocity of follower USV (reverse one time)

0200400600800

100012001400

Tim

e (s)

Latitude ( ∘) Longitude (∘ )31355

3136

3135 120305

1203065

1203055

120307

1203045

120306

(b) Trajectory of follower USV (reverse one time)

Figure 16 The trajectory and speed of follower (reverse two time)

appear around underwater acoustic equipment andinformation cannot be successfully transferred Morestable prediction algorithm or effective compensationis required and the data packet is lost In additionthere are still some other problems such as uncertainnoise and the influence of unknown current

(3) Two new questions appear in front of us The firstone is whether the continuously shortened samplingdata could result in continuously better performanceThe second is whether length of the first stage can beshortened unlimitedly For these two questions we dotwo more simulations the length of the first phase

is taken as 150 seconds and 30 seconds separatelyThe reversion is set as 40 times and 200 timescorrespondingly Initial misalignment estimation andcolocation error convergence curve are shown inFigure 19

From Figure 19 we can see that the estimation accuracycan be improved by shortening the length of sampling dataperiod (such the case of 150 seconds) but the improvement isnot that significant When length of sampling data becomesless than certain value (eg 30 seconds) the estimationaccuracy of the initial misalignment cannot be improvedbut be depressed Then in the second subsection divergent


0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)

(a) Estimation comparison in subsection 1

2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)

(b) Estimation comparison in subsection 2

Figure 17 Estimation comparison

0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)

(a) Colocating error in subsection 1


0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)

(b) Colocating error of the whole process

Figure 18 Comparison of the positioning errors

tendency appears in the estimation of gyro drift and positionerror It is mainly because when time period is too shorteffect of noise would be amplified rapidlyTherefore it is veryimportant to make further study to select the data lengthmore reasonably

7 Summary

In this paper we research on cooperative navigation basedon dead-reckoning with MEMSDVL The nonlinear systemmodel of cooperative navigation is constructed and CKF


0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)

Reverse 200 timesReverse 40 times

Initi

al m

isalig

nmen

t (∘ )

(a) Initial alignment estimation curve

0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)


(b) Colocation error curve

Figure 19 Performance of reverse algorithm with shorter period

is proposed to make information fusion Aiming at thecoexistence of large misalignment and large gyro drift theobservability analysis is carried out On the basis dual-modelestimation idea is introduced to cut off the coupling relationsand improve the observability Furthermore reverse filteringestimation method is proposed The result of simulationand experiments show that it can effectively improve thenavigation algorithm performance The problems in theexperiment are also stated and future research directions arediscussed

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 Natu-ral Science Foundation of China (Grant no 61203225)the State Postdoctoral Science Foundation (2012M510083)the Youth Science Foundation of Heilongjiang Province ofChina (QC2014C069) and the Central College FundamentalResearch Special Fund (ItemnoHEUCF110427)The authorswould like to thank the anonymous reviewers for theirconstructive suggestions and insightful comments

References

[1] C W Park P Ferguson and N Pohlman ldquoDecentralized rela-tive navigation for formation flying spacecraft using augmentedCDGPSrdquo Massachusetts Institute of Technology 2001

[2] A Howard M Matari and G Sukhatme ldquoLocalization formobile robot teams a distributed MLE approachrdquo in Exper-imental Robotics VIII vol 5 of Springer Tracts in AdvancedRobotics pp 146ndash155 Springer Berlin Germany 2003

[3] S I Roumeliotis and G A Bekey ldquoDistributed multi-robotlocalizationrdquo IEEE Transactions on Robotics and Automationvol 18 no 5 pp 781ndash795 2002

[4] RMadhavan K Fregene and L E Parker ldquoDistributed hetero-geneous outdoor multi-robot localizationrdquo in Proceedings of theIEEE International Conference on Robotics and Automation pp374ndash381 Washington DC USA May 2002

[5] B He H Zhang C Li S Zhang Y Liang and T YanldquoAutonomous navigation for autonomous underwater vehiclesbased on information filters and active sensingrdquo Sensors vol 11no 11 pp 10958ndash10980 2011

[6] A BahrM RWalter and J J Leonard ldquoConsistent cooperativelocalizationrdquo inProceedings of the IEEE International Conferenceon Robotics and Automation (ICRA rsquo09) pp 3415ndash3422 KobeJapan May 2009

[7] E T James ldquoThe Navy Unmanned Surface Vehicle (USV)rdquoTech Rep Master Plan NewYork NY USA 2007

[8] R H Battin ldquoSome funny things happened on the way to themoonrdquo Journal of Guidance Control and Dynamics vol 25 no1 pp 1ndash7 2002

[9] S J Julier and J K Uhlmann ldquoUnscented filtering and nonlin-ear estimationrdquo Proceedings of the IEEE vol 92 no 3 pp 401ndash422 2004

[10] W Yuanxin Reseach on Dual-Quaternion Navigation Algo-rithm and Nonlinear Gaussian Filtering National University ofDefense Technology Changsha China 2005

[11] S M HermanA particle filtering approach to joint passive radartracking and target classification [PhD thesis] University ofIllinois at Urbana-Champaign 2002

[12] A Doucet N J Gordon and V Krishnamurthy ldquoParticle filtersfor state estimation of jump Markov linear systemsrdquo IEEE


Transactions on Signal Processing vol 49 no 3 pp 613ndash6242001

[13] Z Chen Bayesian Filtering From Kalman Filters to ParticleFilters and Beyond MeMaster University Hamilton OntarioCanada 2003

[14] I Arasaratnam and S Haykin ldquoCubature kalman filtersrdquo IEEETransactions on Automatic Control vol 54 no 6 pp 1254ndash12692009

[15] D Yole ldquoMEMS front-endmanufacturing trendsrdquoResearch andMarkets vol 3 pp 96ndash114 2013

[16] X Yan C Wenjie and P Wenhui ldquoResearch on random driftmodeling and a Kalman filter based on the differential signal ofMEMS gyroscoperdquo in Proceedings of the 25th Chinese Controland Decision Conference (CCDC 13) pp 3233ndash3237 Guiyang China May 2013

[17] W Li W Wu J Wang and L Lu ldquoA fast SINS initialalignment scheme for underwater vehicle applicationsrdquo Journalof Navigation vol 66 no 2 pp 181ndash198 2013

[18] W Li W Wu J Wang and M Wu ldquoA novel backtrackingnavigation scheme for autonomous underwater vehiclesrdquoMea-surement vol 47 pp 496ndash504 2014

[19] G Antonelli F Arrichiello S Chiaverini and G SukhatmeldquoObservability analysis of relative localization for AUVs basedon ranging and depthmeasurementsrdquo inProceedings of the IEEEInternational Conference on Robotics and Automation (ICRA10) pp 4276ndash4281 Anchorage Alaska USA May 2010

[20] A S Gadre and D J Stilwell ldquoA complete solution to underwa-ter navigation in the presence of unknown currents based onrange measurements from a single locationrdquo in Proceedings ofthe IEEE IRSRSJ International Conference on Intelligent Robotsand Systems (IROS rsquo05) pp 1420ndash1425 August 2005

[21] M Fu Z Deng and J Zhang Kalman Filter Theory and ItsApplication in Navigation Science Press Bei Jing China 2003

[22] S Hong H-H Chun S-H Kwon andM H Lee ldquoObservabil-ity measures and their application to GPSINSrdquo in Proceedingsof the IEEE Transactions on Vehicular Technology pp 97ndash1062008

[23] L Xixiang X Xiaosu W Lihui and L Yiting ldquoA fast compassalignmentmethod for SINS based on saved datardquoMeasurementvol 46 pp 3836ndash3846 2013

[24] Y S Hidaka A I Mourikis and S I Roumeliotis ldquoOptimalformations for cooperative localization of mobile robotsrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo05) pp 4126ndash4131 Barcelona Spain April2005

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


avoid linearization of the nonlinear system by using cubaturepoint sets to approximate the mean and variance Becauseof its high accuracy and low calculation load CKF [14]has become a hotspot in research In this paper CKF isintroduced in the cooperative navigation with nonlinearmodel

As a branch of inertial field inertial systems based onmicroelectromechanical system (MEMS) gained rapid devel-opment in recent years Because of its low-cost small sizelight weight and high reliability there have been more andmore applications in low-cost inertial systems It has becomea preferred solution for the inertial navigation device onfollower boats in multi-USV cooperative network Howeverdue to the limitations of accuracy it is necessary to constructreasonable error model of MEMS based on which estimationand compensation can be preferred [15] Commonly usederror compensation methods include RBF neural networkanalysis ARMA model used in the timing model of gyrorandom error and piecewise interpolation compensation ofscale factor [16] However coexistence of larger initial mis-alignment and larger gyro drift is still unavoidable in practicaluse that leads to poor observability in cooperative navigationBased on different character of initial misalignment and gyrodrift estimation with subsection idea is proposed to cut offthe coupling relations

In addition how to obtain accurate estimation perfor-mance in short time is also a key technique Li et al proposeda new alignment method for AUV with backtracking frame-work The fine alignment runs with the recorded data duringthe process of coarse alignment which effectively improvesthe convergence speed and improve navigation accuracy[17 18] Xixiang Liu also proposed gyrocompass alignmentmethod based on reverse loop to solve compass alignmentfor SINS onmoving base and it greatly reduces the alignmenttime To improve the convergence speed of large initialmisalignment ldquotime reversionrdquo concept is introduced in thispaper As will be confirmed by simulation and experimentalresults the convergence speed and estimation accuracy areimproved greatly

The rest of this paper is organized as follows Section 2presents cooperative navigation model of single master USVand the process of CKF In Section 3 based on observabilityanalysis of cooperative navigation dual-model method andreverse filtering are proposed that improve the convergencespeed and improve the accuracy of cooperative location Sim-ulation of the proposed algorithm is provided in Section 4Aiming at validating the proposed algorithm further theprocess and the results of experiment in Tai Lake are givenin Section 5 Section 6 gives data analysis and discussionsIn Section 7 a summary is provided and future researchdirections are discussed

2 Cooperative Navigation Modeling andProblem Statement

21 Fundamental Theory of Cooperative Navigation ModelingMaster USV and slave USV are equipped with hydroacousticcommunication modem respectively at their bottoms to

achieve information dissemination and distance measure-ments between USVs The masters accomplish navigationusing their high-precision device (usually strap-down inertialnavigation system combined with GPS) while the slaves usetheir own equipped Doppler velocity log (DVL) and attitudeand heading reference system (AHRS) based on MEMS toproceed dead-reckoning

The principle of multi-USV cooperative navigation sys-tem is as follows Firstly master USV sends out fixed fre-quency acoustic pulse signal according to preset time intervalWith this signal slave USV can calculate the relative distanceaccording to underwater acoustic propagation delay Thenmaster USV sends out its position information by hydroa-coustic modem in the form of broadcasting After receivingthis information slave USV can conduct information fusionwith dead-reckoning position ofmasterUSV and the relativedistance and then correct its navigation result using filteringalgorithms Moreover route planning of USV can be carriedout to improve the observability and further improve theaccuracy of the cooperative navigation system

Positioning errors will accumulate over time (as shownby the purple dotted ellipse shown in Figure 1) Therefore itcannot provide accurate positioning information for a longtime When the information sent by master USV is receivedthe slave USV can use cooperative navigation algorithm tocorrect the result of dead-reckoning and effectively reduce thepositioning error (as shown by the green dotted ellipse shownin Figure 1) Through this cyclical filtering and correctionslave USV with low-precision navigation device can realizeprecise location for a long time [19 20]

22 System Model of Single Master USV When the followerdoes not receive the information sent by leaders it can onlyuse its own DVL and low accuracy MEMS to conduct dead-reckoning and themotionmodel can be expressed as follows

119909119878

119896+1= 119909119878

119896+ V119896sdot Δ119905 sdot sin (120572

119896)

119910119878

119896+1= 119910119878

119896+ V119896sdot Δ119905 sdot cos (120572

119896)

(1)

where 119909119878119896 119910119878119896represent the position of follower USV at time

119896 V119896represents the speed Δ119905means the time interval and 120572

119896

means the heading angle at time 119896 However the input modelof system in practice is as follows

V119896= (1 + 120575C

119896) (V119896+ 119908V119896)

120572119896= 119896minus 120575120572119896+ 119908120572119896

(2)

wherein V119896is the velocity measurements and 120575C

119896means the

scale factor error 119896 is the heading anglemeasured byMEMS120575120572119896= Δ120572119896+119896120576Δ119905 is error of

119896Δ120572119896is the initialmisalignment


tk

tkminus1tk+1

Slave USV

Trajectory

Parallel move

Master USV

Trajectory

Slave USV

Slave USVMaster USV

Master USV

Circle 1

Circle 2

Circle 3



119909119878

119896+1= 119909119878

119896+ (1 + 120575C

119896) (V119896+ 119908V119896) sdot Δ119905


120572119896)

119910119878

119896+1= 119910119878

119896+ (1 + 120575C119896) (V119896 + 119908V119896) sdot Δ119905


120575119862119896+1 = 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(3)



of USV at time 119896 u119896= [V119896 119896]

119879 119879(u119896w119896) stands for the


119879119908V119896 sim

119873(0 1205902

V119896) and 119908120572119896 sim 119873(0 1205902


is

Q119896= 119864 w

119896w119879119896 = [

1205902

V1198961205902

120572119896

] (5)


119884119896= 119877119896+ 119881119896= radic(119909

119872

119896minus 119909119878

119896)

2+ (119910119872

119896minus 119910119878

119896)

2+ 119881119896 (6)


119896












120585119894= radic

119898

2

[l]119894

119882119894 =

1

119898

(7)




119875119896minus1|119896minus1

= 119878119896minus1|119896minus1

119878119879

119896minus1|119896minus1 (8a)

119883119894 119896minus1|119896minus1

= 119878119896minus1|119896minus1

120585119894+ 119883119896minus1|119896minus1

(8b)

119883lowast

119894 119896|119896minus1= 119891 (119883

119894 119896minus1|119896minus1) (8c)

119883119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119883lowast

119894 119896|119896minus1 (8d)

119875119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119883lowast

119894 119896|119896minus1119883lowast119879


119883119879

119896|119896minus1+ 119876119896minus1

(8e)


119875119896|119896minus1

= 119878119896|119896minus1

119878119879

119896|119896minus1 (9a)

119883119894 119896|119896minus1

= 119878119896|119896minus1

120585119894+ 119883119896|119896minus1

(9b)

119885119894 119896|119896minus1 = ℎ (119883119894 119896|119896minus1) (9c)

119885119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

(9d)


119875119911119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1+ 1205902

119903 (9e)

119875119909119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119883119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1 (9f)


the state vector 119883119896|119896



119870119896= 119875119909119911

119896|119896minus1(119875119911119911

119896|119896minus1)

minus1

(10a)

119883119896|119896= 119883119896|119896minus1

+ 119870119896(119884119896minus119885119896|119896minus1

) (10b)

119875119896|119896= 119875119896|119896minus1

minus 119870119896119875119911119911

119896|119896minus1119870119879

119896 (10c)




119877119896+1 = ((119909119878


119872

119896+1)

2

+ (119910119878

119896+ V119896sdot Δ119905 sdot cos (

119896minus 120575120572119896+ 119908120572119896) minus 119910119872

119896+1)

2

)

12

(11)


120575120572119896= arcsin

(119877119896+1)2minus (119909119878

119896minus 119909119872

119896+1)

2

minus (119910119878

119896minus 119910119872

119896+1)

2

minus V2119896Δ1199052

2V119896Δ119905radic(119909

119878

119896minus 119909119872

119896+1)

2

+ (119910119878

119896minus 119910119872

119896+1)

2


(12)


119896+1)(119909119878

119896minus 119909119872

119896+1)) So 120575120572

119896can






119896= [119909119878

119896119910119878

119896Δ120572119896120576119896120575119862119896]119879 and 120575120572

119896= Δ120572119896+ 120576 sdot 119896 sdot


119909119878

119896+1=119909119878


119910119878

119896+1=119910119878

119896+ (1 + 120575C


120575119862119896+1= 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(13)


F119896=

120597119891

120597X119896

=

[

[

[

[

[

[

[

[


0 1 V119896Δ119905 sin (

119896minus 120575120572119896+ 119908120572119896) V119896119896Δ1199052 sin (

119896minus 120575120572119896+ 119908120572119896) (V

119896+ 119908V119896) Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896)

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1

]

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X119896

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0 0] = [

Δ119909119896

119877119896

Δ119910119896

119877119896

0 0 0] = [sin 119896 cos 1198960 0 0]

(14)

where Δ119909119896= 119909119878

119896minus 119909119872

119896 Δ119910119896= 119910119878

119896minus 119910119872

119896



Γ =

[

[

[

[

[

[

H119896


F119896minus4

]

]

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0 0


1015840


2 sin1205721015840119896minus1


119896minus1



2 sin1205721015840119896minus2


119896minus2



2 sin1205721015840119896minus3


119896minus3



2 sin1205721015840119896minus4


119896minus4

]

]

]

]

]

]

]

]

]

]

]

]

]

]

]

(15)

10 20 30 40 50 60 70 80 90 100 110 1200

01

02

03

04

05

06

07

08

09

1

100 102 104 106 108 110 112 114 116 118 1200008

00085

0009

00095

001

00105

0011

00115

0012

k

y

y1 = 1 (k minus 1)



where 1205721015840119895= (120575120572119895 minus 120596120572119895







Subsection 1 X1198961= [119909119878

119896119910119878

119896Δ120572119896120575119862119896]

119879

Subsection 2 X1198962= [119909119878

119896119910119878

119896120576119896120575119862119896]

119879

(16)


1198961=

[119909119878

119896119910119878

119896Δ120572119896 120575119862119896

]

119879


F119896=

120597119891

1205971198831198961

=

[

[

[

[

[

[

[



0 0 1 0

0 0 0 1

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X1198961

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0] = [sin 119896 cos 1198960 0]

(17)



Γ =

[

[

[

[

H119896

H119896minus1

F119896minus1

H119896minus2

F119896minus2

H119896minus3

F119896minus3

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0

sin 119896minus1

cos 119896minus1



sin 119896minus2

cos 119896minus2



sin 119896minus3

cos 119896minus3



]

]

]

]

]

]

]

]

]

]

(18)




100 09970 00030200 09939 00061300 09909 00091400 09878 00122500 09848 00152









119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1 = Δ120572119896

(19)


119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1= Δ120572119896

(20)



k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)


t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n


0 5000

01

02

03

04

05

06

07

08

09

1

t (s)



2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










tk

tkminus1tk+1

Slave USV

Trajectory

Parallel move

Master USV

Trajectory

Slave USV

Slave USVMaster USV

Master USV

Circle 1

Circle 2

Circle 3



119909119878

119896+1= 119909119878

119896+ (1 + 120575C

119896) (V119896+ 119908V119896) sdot Δ119905


120572119896)

119910119878

119896+1= 119910119878

119896+ (1 + 120575C119896) (V119896 + 119908V119896) sdot Δ119905


120575119862119896+1 = 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(3)



of USV at time 119896 u119896= [V119896 119896]

119879 119879(u119896w119896) stands for the


119879119908V119896 sim

119873(0 1205902

V119896) and 119908120572119896 sim 119873(0 1205902


is

Q119896= 119864 w

119896w119879119896 = [

1205902

V1198961205902

120572119896

] (5)


119884119896= 119877119896+ 119881119896= radic(119909

119872

119896minus 119909119878

119896)

2+ (119910119872

119896minus 119910119878

119896)

2+ 119881119896 (6)


119896












120585119894= radic

119898

2

[l]119894

119882119894 =

1

119898

(7)




119875119896minus1|119896minus1

= 119878119896minus1|119896minus1

119878119879

119896minus1|119896minus1 (8a)

119883119894 119896minus1|119896minus1

= 119878119896minus1|119896minus1

120585119894+ 119883119896minus1|119896minus1

(8b)

119883lowast

119894 119896|119896minus1= 119891 (119883

119894 119896minus1|119896minus1) (8c)

119883119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119883lowast

119894 119896|119896minus1 (8d)

119875119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119883lowast

119894 119896|119896minus1119883lowast119879


119883119879

119896|119896minus1+ 119876119896minus1

(8e)


119875119896|119896minus1

= 119878119896|119896minus1

119878119879

119896|119896minus1 (9a)

119883119894 119896|119896minus1

= 119878119896|119896minus1

120585119894+ 119883119896|119896minus1

(9b)

119885119894 119896|119896minus1 = ℎ (119883119894 119896|119896minus1) (9c)

119885119896|119896minus1

=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

(9d)


119875119911119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1+ 1205902

119903 (9e)

119875119909119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119883119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1 (9f)


the state vector 119883119896|119896



119870119896= 119875119909119911

119896|119896minus1(119875119911119911

119896|119896minus1)

minus1

(10a)

119883119896|119896= 119883119896|119896minus1

+ 119870119896(119884119896minus119885119896|119896minus1

) (10b)

119875119896|119896= 119875119896|119896minus1

minus 119870119896119875119911119911

119896|119896minus1119870119879

119896 (10c)




119877119896+1 = ((119909119878


119872

119896+1)

2

+ (119910119878

119896+ V119896sdot Δ119905 sdot cos (

119896minus 120575120572119896+ 119908120572119896) minus 119910119872

119896+1)

2

)

12

(11)


120575120572119896= arcsin

(119877119896+1)2minus (119909119878

119896minus 119909119872

119896+1)

2

minus (119910119878

119896minus 119910119872

119896+1)

2

minus V2119896Δ1199052

2V119896Δ119905radic(119909

119878

119896minus 119909119872

119896+1)

2

+ (119910119878

119896minus 119910119872

119896+1)

2


(12)


119896+1)(119909119878

119896minus 119909119872

119896+1)) So 120575120572

119896can






119896= [119909119878

119896119910119878

119896Δ120572119896120576119896120575119862119896]119879 and 120575120572

119896= Δ120572119896+ 120576 sdot 119896 sdot


119909119878

119896+1=119909119878


119910119878

119896+1=119910119878

119896+ (1 + 120575C


120575119862119896+1= 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(13)


F119896=

120597119891

120597X119896

=

[

[

[

[

[

[

[

[


0 1 V119896Δ119905 sin (

119896minus 120575120572119896+ 119908120572119896) V119896119896Δ1199052 sin (

119896minus 120575120572119896+ 119908120572119896) (V

119896+ 119908V119896) Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896)

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1

]

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X119896

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0 0] = [

Δ119909119896

119877119896

Δ119910119896

119877119896

0 0 0] = [sin 119896 cos 1198960 0 0]

(14)

where Δ119909119896= 119909119878

119896minus 119909119872

119896 Δ119910119896= 119910119878

119896minus 119910119872

119896



Γ =

[

[

[

[

[

[

H119896


F119896minus4

]

]

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0 0


1015840


2 sin1205721015840119896minus1


119896minus1



2 sin1205721015840119896minus2


119896minus2



2 sin1205721015840119896minus3


119896minus3



2 sin1205721015840119896minus4


119896minus4

]

]

]

]

]

]

]

]

]

]

]

]

]

]

]

(15)

10 20 30 40 50 60 70 80 90 100 110 1200

01

02

03

04

05

06

07

08

09

1

100 102 104 106 108 110 112 114 116 118 1200008

00085

0009

00095

001

00105

0011

00115

0012

k

y

y1 = 1 (k minus 1)



where 1205721015840119895= (120575120572119895 minus 120596120572119895







Subsection 1 X1198961= [119909119878

119896119910119878

119896Δ120572119896120575119862119896]

119879

Subsection 2 X1198962= [119909119878

119896119910119878

119896120576119896120575119862119896]

119879

(16)


1198961=

[119909119878

119896119910119878

119896Δ120572119896 120575119862119896

]

119879


F119896=

120597119891

1205971198831198961

=

[

[

[

[

[

[

[



0 0 1 0

0 0 0 1

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X1198961

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0] = [sin 119896 cos 1198960 0]

(17)



Γ =

[

[

[

[

H119896

H119896minus1

F119896minus1

H119896minus2

F119896minus2

H119896minus3

F119896minus3

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0

sin 119896minus1

cos 119896minus1



sin 119896minus2

cos 119896minus2



sin 119896minus3

cos 119896minus3



]

]

]

]

]

]

]

]

]

]

(18)




100 09970 00030200 09939 00061300 09909 00091400 09878 00122500 09848 00152









119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1 = Δ120572119896

(19)


119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1= Δ120572119896

(20)



k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)


t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n


0 5000

01

02

03

04

05

06

07

08

09

1

t (s)



2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










119875119911119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119885119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1+ 1205902

119903 (9e)

119875119909119911

119896|119896minus1=

1

2119873

2119873

sum

119894=1

119883119894 119896|119896minus1

119885119879


119885119879

119896|119896minus1 (9f)


the state vector 119883119896|119896



119870119896= 119875119909119911

119896|119896minus1(119875119911119911

119896|119896minus1)

minus1

(10a)

119883119896|119896= 119883119896|119896minus1

+ 119870119896(119884119896minus119885119896|119896minus1

) (10b)

119875119896|119896= 119875119896|119896minus1

minus 119870119896119875119911119911

119896|119896minus1119870119879

119896 (10c)




119877119896+1 = ((119909119878


119872

119896+1)

2

+ (119910119878

119896+ V119896sdot Δ119905 sdot cos (

119896minus 120575120572119896+ 119908120572119896) minus 119910119872

119896+1)

2

)

12

(11)


120575120572119896= arcsin

(119877119896+1)2minus (119909119878

119896minus 119909119872

119896+1)

2

minus (119910119878

119896minus 119910119872

119896+1)

2

minus V2119896Δ1199052

2V119896Δ119905radic(119909

119878

119896minus 119909119872

119896+1)

2

+ (119910119878

119896minus 119910119872

119896+1)

2


(12)


119896+1)(119909119878

119896minus 119909119872

119896+1)) So 120575120572

119896can






119896= [119909119878

119896119910119878

119896Δ120572119896120576119896120575119862119896]119879 and 120575120572

119896= Δ120572119896+ 120576 sdot 119896 sdot


119909119878

119896+1=119909119878


119910119878

119896+1=119910119878

119896+ (1 + 120575C


120575119862119896+1= 120575119862119896

Δ120572119896+1= Δ120572119896

120576119896+1= 120576119896

(13)


F119896=

120597119891

120597X119896

=

[

[

[

[

[

[

[

[


0 1 V119896Δ119905 sin (

119896minus 120575120572119896+ 119908120572119896) V119896119896Δ1199052 sin (

119896minus 120575120572119896+ 119908120572119896) (V

119896+ 119908V119896) Δ119905 cos (119896 minus 120575120572119896 + 119908120572119896)

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1

]

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X119896

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0 0] = [

Δ119909119896

119877119896

Δ119910119896

119877119896

0 0 0] = [sin 119896 cos 1198960 0 0]

(14)

where Δ119909119896= 119909119878

119896minus 119909119872

119896 Δ119910119896= 119910119878

119896minus 119910119872

119896



Γ =

[

[

[

[

[

[

H119896


F119896minus4

]

]

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0 0


1015840


2 sin1205721015840119896minus1


119896minus1



2 sin1205721015840119896minus2


119896minus2



2 sin1205721015840119896minus3


119896minus3



2 sin1205721015840119896minus4


119896minus4

]

]

]

]

]

]

]

]

]

]

]

]

]

]

]

(15)

10 20 30 40 50 60 70 80 90 100 110 1200

01

02

03

04

05

06

07

08

09

1

100 102 104 106 108 110 112 114 116 118 1200008

00085

0009

00095

001

00105

0011

00115

0012

k

y

y1 = 1 (k minus 1)



where 1205721015840119895= (120575120572119895 minus 120596120572119895







Subsection 1 X1198961= [119909119878

119896119910119878

119896Δ120572119896120575119862119896]

119879

Subsection 2 X1198962= [119909119878

119896119910119878

119896120576119896120575119862119896]

119879

(16)


1198961=

[119909119878

119896119910119878

119896Δ120572119896 120575119862119896

]

119879


F119896=

120597119891

1205971198831198961

=

[

[

[

[

[

[

[



0 0 1 0

0 0 0 1

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X1198961

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0] = [sin 119896 cos 1198960 0]

(17)



Γ =

[

[

[

[

H119896

H119896minus1

F119896minus1

H119896minus2

F119896minus2

H119896minus3

F119896minus3

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0

sin 119896minus1

cos 119896minus1



sin 119896minus2

cos 119896minus2



sin 119896minus3

cos 119896minus3



]

]

]

]

]

]

]

]

]

]

(18)




100 09970 00030200 09939 00061300 09909 00091400 09878 00122500 09848 00152









119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1 = Δ120572119896

(19)


119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1= Δ120572119896

(20)



k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)


t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n


0 5000

01

02

03

04

05

06

07

08

09

1

t (s)



2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra











Γ =

[

[

[

[

[

[

H119896


F119896minus4

]

]

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0 0


1015840


2 sin1205721015840119896minus1


119896minus1



2 sin1205721015840119896minus2


119896minus2



2 sin1205721015840119896minus3


119896minus3



2 sin1205721015840119896minus4


119896minus4

]

]

]

]

]

]

]

]

]

]

]

]

]

]

]

(15)

10 20 30 40 50 60 70 80 90 100 110 1200

01

02

03

04

05

06

07

08

09

1

100 102 104 106 108 110 112 114 116 118 1200008

00085

0009

00095

001

00105

0011

00115

0012

k

y

y1 = 1 (k minus 1)



where 1205721015840119895= (120575120572119895 minus 120596120572119895







Subsection 1 X1198961= [119909119878

119896119910119878

119896Δ120572119896120575119862119896]

119879

Subsection 2 X1198962= [119909119878

119896119910119878

119896120576119896120575119862119896]

119879

(16)


1198961=

[119909119878

119896119910119878

119896Δ120572119896 120575119862119896

]

119879


F119896=

120597119891

1205971198831198961

=

[

[

[

[

[

[

[



0 0 1 0

0 0 0 1

]

]

]

]

]

]

]

H119896=

120597ℎ [sdot]

120597X1198961

= [

119909119878

119896minus 119909119872

119896

119877119896

119910119878

119896minus 119910119872

119896

119877119896

0 0] = [sin 119896 cos 1198960 0]

(17)



Γ =

[

[

[

[

H119896

H119896minus1

F119896minus1

H119896minus2

F119896minus2

H119896minus3

F119896minus3

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0

sin 119896minus1

cos 119896minus1



sin 119896minus2

cos 119896minus2



sin 119896minus3

cos 119896minus3



]

]

]

]

]

]

]

]

]

]

(18)




100 09970 00030200 09939 00061300 09909 00091400 09878 00122500 09848 00152









119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1 = Δ120572119896

(19)


119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1= Δ120572119896

(20)



k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)


t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n


0 5000

01

02

03

04

05

06

07

08

09

1

t (s)



2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra











Γ =

[

[

[

[

H119896

H119896minus1

F119896minus1

H119896minus2

F119896minus2

H119896minus3

F119896minus3

]

]

]

]

=

[

[

[

[

[

[

[

[

[

[

sin 119896

cos 119896

0 0

sin 119896minus1

cos 119896minus1



sin 119896minus2

cos 119896minus2



sin 119896minus3

cos 119896minus3



]

]

]

]

]

]

]

]

]

]

(18)




100 09970 00030200 09939 00061300 09909 00091400 09878 00122500 09848 00152









119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1 = Δ120572119896

(19)


119878

119896+1=119878

119896+ (1 + 120575C


119910119878

119896+1= 119910119878

119896+ (1 + 120575C


120575C119896+1= 120575C119896

Δ120572119896+1= Δ120572119896

(20)



k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)


t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n


0 5000

01

02

03

04

05

06

07

08

09

1

t (s)



2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










k (s)

Δ120572

(∘)

(a)

k (s)

kmiddot120576

middotΔt

(∘)

(b)


t (h)0 2 4 6 8 10 12

0

01

02

03

04

05

06

07

08

09

1

Prop

ortio

n


0 5000

01

02

03

04

05

06

07

08

09

1

t (s)



2

1

3

2n + 1

larrVk = minus

rarrVk

rarrV1 = minus

larrV1











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra











and reinitialize


Forwards

rarrxS

k+1 =rarrxS

k + (1 + 120575Ck)(rarr k + w119896


rarryS

k+1 =rarryS

k + (1 + 120575Ck)(rarr k + w119896


120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Converse

larrxS

k+1 =larr larr

larr

xS


)larryS

k+1 =larryS


)120575Ck+1 = 120575Ck

Δ120572k+1 = Δ120572k

Forwards

120575Ck+1 = 120575Ck

120576k+1 = 120576k

)))

)))

)))

)))

)))

)))


)


)

z1 z2 zm zm+1 zm+2


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

1000

2000

3000

4000

5000

6000

0 500 10000

200

400

600

800


X axis (m)

Yax

is (m

)

y-a

xis (

m)

x-axis (m)




0 1000 2000 3000 4000 5000 60000

10

20

30

40

50

60

70

80

90

100



Estim

atio

n



t (s)



=




200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










200 400 600 800 1000 1200 140089

895

90

905

91

915

92


Initi

al m

isalig

nmen

t (∘ )

t (s)


0 1 2 3 4 5 60

1

2

3

4

5

6

7

8

9

10

times104t (s)

Gyr

o dr

ift (∘

h)


0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160


Time (s)

Posit

ion

erro

r (m

)











loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










loci

ty (m

s)

Time (s)

0

1000

2000 3000 4000 5000 6000

0

5

10

15

20

0 5000400030002000500 0 500

1000minus20

minus15

minus10

minus5


Velo

city

(ms

)

Time (s)

0 1000 2000 3000 4000 5000 60000 480038002800300 0 3000 300 800 1800

0

5

10

15

20

minus20

minus15

minus10

minus5















Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










Time (s)

Initi

al m

isalig

nmen

t and

gyr

o dr

ift es

timat

ion

1000 2000 3000 4000 5000 6000

10

20

30

40

50

60

70

80

90


100000

5000400030002000500 0 5000 480038002800300 0 3000 300 800 1800


Time (s)

895

89

90

905

91

915

92


400 600 800 1000 1200 1400200200 500 0 500200 300 0 300 0 300

Initi

al m

isalig

nmen

t esti

mat

ion

(∘)


Time (s)

5000500300 4800

1500 2500 3500 4500 60006

7

8

9

10

11

12

Without reverse


1500 2500 35001300 2300 3300

Gyr

o dr

ift es

timat

ion

(∘h

)


Posit

ion

erro

r (m

)

0 0500 500 1000 2000 3000 4000 5000

0 300 0 300 0 300 800 1800 2800 3800 4800

0 1000 2000 3000 4000 5000 60000

20

40

60

80

100

120

140

160

Without reverse


Time (s)







PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra











PHINS

MEMS

Acoustic device DVL

UPS power

(a) (b)


Acoustic sender 1

Acoustic sender 2


GPS 2GPS 1

Position of A


MEMS


PHINS



GPS3

Integrated system

Joint installation

panel

USB USB Serial port

USB

USBUSB


Serial port

Follower USV (C)

Serial port

Acoustic receiver 1

Acoustic receiver 2


Position of B










MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra











MEMS







Latit

ude (

∘ )

Longitude (∘)

Follower

3144

3142

314

3138

3136

3134

3132

313

3128

31261201 12015 1202 12025 1203 12035

Leader 1Leader 2








6 Discussion





0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










0 2000 4000 6000 8000 10000 12000

0

2

4

6

8

10

12

31670 11167916771675167417 1167

1250minus8

minus6

minus4

minus2

Time (s)

Velo

city

(ms

)


0

200

400

600

800

1000

1200

1400

Tim

e (s)


313631359

3135831357

31356 120298120302

12030612031



0 2000 4000 6000 8000 10000 120001250

30000 11000900070005000250 1000Time (s)

0

2

4

6

8

10

12

minus8

minus6

minus4

minus2

Velo

city

(ms

)


0200400600800

100012001400

Tim

e (s)


3136

3135 120305

1203065

1203055

120307

1203045

120306








0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










0 200 400 600 800 1000 12000

10

20

30

40

50

60

70

80

90


4170 41702500 2500 0250

Time (s)

Initi

al er

ror a

ngle

(∘)


2000 3000 4000 5000 6000 7000 8000 9000 10000

0

20

40

60

80

Without reverse


417 9167

250 9000

1250

1167 2167 3167 4167 5167 6167 7167 8167

1000 2000 3000 4000 5000 6000 7000 8000

minus20

minus40

Time (s)

Gyr

o dr

ift (∘h

)



0 200 400 600 800 1000 1200 14000

50

100

150

200

250


4170 4170

2500 2500 0250

567400

Posit

ion

erro

r (m

)

Time (s)



0 2000 4000 6000 8000 10000 120000

100

200

300

400

500

600

0 1167 3167 5167 7167 9167 111671000 3000 5000 7000 9000 11000

Posit

ion

erro

r (m

)

Time (s)




7 Summary



0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra










0 2000 4000 6000 8000 10000 12000 140000

10

20

30

40

50

60

70

80

90

100

Time (s)


Initi

al m

isalig

nmen

t (∘ )


0 5000 10000 150000

20

40

60

80

100

120

140

160

Posit

ion

erro

r (m

)

Time (s)







Acknowledgments


References


































Volume 2014




Journal of











Journal of


Function Spaces






Algebra






























Volume 2014




Journal of











Journal of


Function Spaces






Algebra









research article dual-model reverse ckf algorithm in

Documents