Research Article Dual-Model Reverse CKF Algorithm in ...
Transcript of 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
119896) represents the posi-
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)
4 Mathematical Problems in Engineering
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
119894 119896|119896minus1minus 119883119896|119896minus1
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
Mathematical Problems in Engineering 5
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
sin 119896minus2 cos 119896minus2 minusV119896minus2Δ119905 sin1205721015840
119896minus2minusV119896minus2 (119896 minus 2) Δ119905
2 sin1205721015840119896minus2
(V119896minus2 + 119908V119896minus2) Δ119905 cos1205721015840
119896minus2
sin 119896minus3 cos 119896minus3 minusV119896minus3Δ119905 sin1205721015840
119896minus3minusV119896minus3 (119896 minus 3) Δ119905
2 sin1205721015840119896minus3
(V119896minus3 + 119908V119896minus3) Δ119905 cos1205721015840
119896minus3
sin 119896minus4 cos 119896minus4 minusV119896minus4Δ119905 sin1205721015840
119896minus4minusV119896minus4 (119896 minus 4) Δ119905
2 sin1205721015840119896minus4
(V119896minus4 + 119908V119896minus4) Δ119905 cos1205721015840
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)
6 Mathematical Problems in Engineering
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
minusV119896minus2Δ119905 sin (120575120572
119896minus2minus 119908120572119896minus2) (V119896minus2+ 119908V119896minus2) Δ119905 cos (120575120572119896minus2 minus 119908120572119896minus2)
sin 119896minus3
cos 119896minus3
minusV119896minus3Δ119905 sin (120575120572
119896minus3minus 119908120572119896minus3) (V119896minus3+ 119908V119896minus3) Δ119905 cos (120575120572119896minus3 minus 119908120572119896minus3)
]
]
]
]
]
]
]
]
]
]
(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
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
(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
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
2 Mathematical Problems in Engineering
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
119896) represents the posi-
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)
4 Mathematical Problems in Engineering
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
119894 119896|119896minus1minus 119883119896|119896minus1
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
Mathematical Problems in Engineering 5
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
sin 119896minus2 cos 119896minus2 minusV119896minus2Δ119905 sin1205721015840
119896minus2minusV119896minus2 (119896 minus 2) Δ119905
2 sin1205721015840119896minus2
(V119896minus2 + 119908V119896minus2) Δ119905 cos1205721015840
119896minus2
sin 119896minus3 cos 119896minus3 minusV119896minus3Δ119905 sin1205721015840
119896minus3minusV119896minus3 (119896 minus 3) Δ119905
2 sin1205721015840119896minus3
(V119896minus3 + 119908V119896minus3) Δ119905 cos1205721015840
119896minus3
sin 119896minus4 cos 119896minus4 minusV119896minus4Δ119905 sin1205721015840
119896minus4minusV119896minus4 (119896 minus 4) Δ119905
2 sin1205721015840119896minus4
(V119896minus4 + 119908V119896minus4) Δ119905 cos1205721015840
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)
6 Mathematical Problems in Engineering
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
minusV119896minus2Δ119905 sin (120575120572
119896minus2minus 119908120572119896minus2) (V119896minus2+ 119908V119896minus2) Δ119905 cos (120575120572119896minus2 minus 119908120572119896minus2)
sin 119896minus3
cos 119896minus3
minusV119896minus3Δ119905 sin (120575120572
119896minus3minus 119908120572119896minus3) (V119896minus3+ 119908V119896minus3) Δ119905 cos (120575120572119896minus3 minus 119908120572119896minus3)
]
]
]
]
]
]
]
]
]
]
(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
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
(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
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 3
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
119896) represents the posi-
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)
4 Mathematical Problems in Engineering
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
119894 119896|119896minus1minus 119883119896|119896minus1
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
Mathematical Problems in Engineering 5
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
sin 119896minus2 cos 119896minus2 minusV119896minus2Δ119905 sin1205721015840
119896minus2minusV119896minus2 (119896 minus 2) Δ119905
2 sin1205721015840119896minus2
(V119896minus2 + 119908V119896minus2) Δ119905 cos1205721015840
119896minus2
sin 119896minus3 cos 119896minus3 minusV119896minus3Δ119905 sin1205721015840
119896minus3minusV119896minus3 (119896 minus 3) Δ119905
2 sin1205721015840119896minus3
(V119896minus3 + 119908V119896minus3) Δ119905 cos1205721015840
119896minus3
sin 119896minus4 cos 119896minus4 minusV119896minus4Δ119905 sin1205721015840
119896minus4minusV119896minus4 (119896 minus 4) Δ119905
2 sin1205721015840119896minus4
(V119896minus4 + 119908V119896minus4) Δ119905 cos1205721015840
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)
6 Mathematical Problems in Engineering
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
minusV119896minus2Δ119905 sin (120575120572
119896minus2minus 119908120572119896minus2) (V119896minus2+ 119908V119896minus2) Δ119905 cos (120575120572119896minus2 minus 119908120572119896minus2)
sin 119896minus3
cos 119896minus3
minusV119896minus3Δ119905 sin (120575120572
119896minus3minus 119908120572119896minus3) (V119896minus3+ 119908V119896minus3) Δ119905 cos (120575120572119896minus3 minus 119908120572119896minus3)
]
]
]
]
]
]
]
]
]
]
(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
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
(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
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
4 Mathematical Problems in Engineering
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
119894 119896|119896minus1minus 119883119896|119896minus1
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
Mathematical Problems in Engineering 5
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
sin 119896minus2 cos 119896minus2 minusV119896minus2Δ119905 sin1205721015840
119896minus2minusV119896minus2 (119896 minus 2) Δ119905
2 sin1205721015840119896minus2
(V119896minus2 + 119908V119896minus2) Δ119905 cos1205721015840
119896minus2
sin 119896minus3 cos 119896minus3 minusV119896minus3Δ119905 sin1205721015840
119896minus3minusV119896minus3 (119896 minus 3) Δ119905
2 sin1205721015840119896minus3
(V119896minus3 + 119908V119896minus3) Δ119905 cos1205721015840
119896minus3
sin 119896minus4 cos 119896minus4 minusV119896minus4Δ119905 sin1205721015840
119896minus4minusV119896minus4 (119896 minus 4) Δ119905
2 sin1205721015840119896minus4
(V119896minus4 + 119908V119896minus4) Δ119905 cos1205721015840
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)
6 Mathematical Problems in Engineering
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
minusV119896minus2Δ119905 sin (120575120572
119896minus2minus 119908120572119896minus2) (V119896minus2+ 119908V119896minus2) Δ119905 cos (120575120572119896minus2 minus 119908120572119896minus2)
sin 119896minus3
cos 119896minus3
minusV119896minus3Δ119905 sin (120575120572
119896minus3minus 119908120572119896minus3) (V119896minus3+ 119908V119896minus3) Δ119905 cos (120575120572119896minus3 minus 119908120572119896minus3)
]
]
]
]
]
]
]
]
]
]
(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
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
(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
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 5
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
sin 119896minus2 cos 119896minus2 minusV119896minus2Δ119905 sin1205721015840
119896minus2minusV119896minus2 (119896 minus 2) Δ119905
2 sin1205721015840119896minus2
(V119896minus2 + 119908V119896minus2) Δ119905 cos1205721015840
119896minus2
sin 119896minus3 cos 119896minus3 minusV119896minus3Δ119905 sin1205721015840
119896minus3minusV119896minus3 (119896 minus 3) Δ119905
2 sin1205721015840119896minus3
(V119896minus3 + 119908V119896minus3) Δ119905 cos1205721015840
119896minus3
sin 119896minus4 cos 119896minus4 minusV119896minus4Δ119905 sin1205721015840
119896minus4minusV119896minus4 (119896 minus 4) Δ119905
2 sin1205721015840119896minus4
(V119896minus4 + 119908V119896minus4) Δ119905 cos1205721015840
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)
6 Mathematical Problems in Engineering
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
minusV119896minus2Δ119905 sin (120575120572
119896minus2minus 119908120572119896minus2) (V119896minus2+ 119908V119896minus2) Δ119905 cos (120575120572119896minus2 minus 119908120572119896minus2)
sin 119896minus3
cos 119896minus3
minusV119896minus3Δ119905 sin (120575120572
119896minus3minus 119908120572119896minus3) (V119896minus3+ 119908V119896minus3) Δ119905 cos (120575120572119896minus3 minus 119908120572119896minus3)
]
]
]
]
]
]
]
]
]
]
(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
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
(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
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
6 Mathematical Problems in Engineering
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
minusV119896minus2Δ119905 sin (120575120572
119896minus2minus 119908120572119896minus2) (V119896minus2+ 119908V119896minus2) Δ119905 cos (120575120572119896minus2 minus 119908120572119896minus2)
sin 119896minus3
cos 119896minus3
minusV119896minus3Δ119905 sin (120575120572
119896minus3minus 119908120572119896minus3) (V119896minus3+ 119908V119896minus3) Δ119905 cos (120575120572119896minus3 minus 119908120572119896minus3)
]
]
]
]
]
]
]
]
]
]
(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
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
(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
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 7
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
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
8 Mathematical Problems in Engineering
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)
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 9
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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
10 Mathematical Problems in 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
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 11
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
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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)
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
12 Mathematical Problems in Engineering
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
Data collecting system 3
PHINS
Navigation information
Navigation information
GPS3
Integrated system
Joint installation
panel
USB USB Serial port
USB
USBUSB
Data collecting system 2
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
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 13
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
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
14 Mathematical Problems in Engineering
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
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 15
0 200 400 600 800 1000 12000
10
20
30
40
50
60
70
80
90
Without reverseReverse onceReverse twice
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
Reverse onceReverse twice
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
Without reverseReverse onceReverse twice
4170 4170
2500 2500 0250
567400
Posit
ion
erro
r (m
)
Time (s)
(a) Colocating error in subsection 1
Without reverseReverse onceReverse twice
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
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
16 Mathematical Problems in Engineering
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)
Reverse 200 timesReverse 40 times
(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
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Mathematical Problems in Engineering 17
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
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of
Submit your manuscripts athttpwwwhindawicom
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttpwwwhindawicom
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttpwwwhindawicom
Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014
Stochastic AnalysisInternational Journal of