DTIC ADA537319: Asynchronous Data Fusion for AUV Navigation Via Heuristic Fuzzy Filtering Techniques

7 Pages·0.56 MB·English

by  Defense Technical Information Center

Asynchronous Data Fusion For AUV Navigation Via
Heuristic Fuzzy Filtering Techniques
P. E. An z, A. J. Healey y, J. Park z, S. M. Smith z
zDepartment of Ocean Engineering
Florida Atlantic University, Boca Raton, FL 33431
Email: [email protected]
yDepartment of Mechanical Engineering
Naval Postgraduate School, Monterey, CA 93943
Abstract
This paper presents a heuristic fuzzy position estimation technique for autonomous un-
derwater vehicle navigation. The heuristic estimator performs asynchronous data fusion
ofall sensor measurements based ontheir relative con(cid:12)dence levels, andthen nonlinearly
combines the fused informationwith the INS estimates via fuzzy (cid:12)ltering techniques. In
this paper, the basis and implementation of the estimator will be described, and naviga-
tion results will be presented based on the heuristic estimator. In addition, performance
comparisonbased on the heuristic estimator andthose based on extended Kalman(cid:12)lters
will be reported inourcompanionpaper, andthe results are expected toprovideinsights
intotheprosandconsofindividualmethodsinterms ofcomputationalcost, steady-state
andconvergence characteristics for biasestimation.
1 Introduction
With rapid progress in COTS sensors and electronics proach with respect to steady-state and convergence
technology, miniaturized autonomous underwater ve- performance of bias estimation. This evaluation will
hicles (AUV) have reached an acceptable level of ma- be reported in our companion paper. This paper is
turity andreliability which canbe capitalized ontheir organized asfollows: the next section brie(cid:13)y describes
use for oceanographic and military applications [5, 6]. the sensorandcapability ofthe Ocean Explorer AUV.
Examplesincludespatio-temporalsurveysduringclan- Section3presentstheheuristicnavigationarchitecture
destine oceanographic and mine counter-measure op- and algorithms currently implemented on the OEX.
erations in shallow-water environments. Without re- Section 4 presents navigation results based on GPS-
quiring any tethering support, the dynamic stability doppler aided INS system, and (cid:12)nally the last section
and data sampling quality can be much improved. In provides concluding remarks.
addition, multiple small AUVs canbe deployed simul-
taneously to traverse in di(cid:11)erent regions without ne-
cessitating one-to-onesurfaceships,andthis results in
2 Ocean Explorer
higher data sampling e(cid:14)ciency [7]. To truly charac-
terize four-dimensonalocean dynamics autonomously,
high-precision underwater navigation is a technically TheOEXAUV is asmallanduntethered autonomous
challenging issue because vehicle localization must be underwater vehicle. It is 7 ft long (extendable to 10
done onboard. While di(cid:11)erential GPS sensor technol- ft) and maximum diameter is 21". The hull is based
ogy provides a good solution for surface navigation on a modi(cid:12)ed Gertler’s Series 58 Model 4154 with
[2], underwater navigation still remains a challenging a modular mid-section for interfacing swappable pay-
problem especially when sonar position beacons are loads. Thecruciformcontrolsurfacesareaft-mounted,
unavailable. Themaingoalofthispaperistopresenta and are replaceable with larger (cid:12)ns if a 3’ mid-section
novel heuristic datafusionwhich collates di(cid:11)erent and is inserted. In air, the vehicle weighs approximately
independent asynchronous position sensors together, 400lbs, and is designed to be neutrally buoyant. The
andnonlinearly gain schedules themwith the onboard OEX cruises at a speed of 3 knots (a range of 2-5
INS system. One important objective of our study knots), and can execute any pre-programmedmission
is to evaluate the e(cid:11)ectiveness of the algorithmic per- continuouslyforapproximately12hoursbeforeNi-Cad
formance based on computationally-intensive model- batteries are to be re-charged. Note that the vehi-
basedapproach[3,8]versusheuristic sensor-basedap- cle mission can be re-programmed while the OEX is
1
0-7803-4111-2/97 $10.00 (c) 1997 IEEE
Report Documentation Page Form Approved
OMB No. 0704-0188
Public reporting burden for the collection of information is estimated to average 1 hour per response, including the time for reviewing instructions, searching existing data sources, gathering and
maintaining the data needed, and completing and reviewing the collection of information. Send comments regarding this burden estimate or any other aspect of this collection of information,
including suggestions for reducing this burden, to Washington Headquarters Services, Directorate for Information Operations and Reports, 1215 Jefferson Davis Highway, Suite 1204, Arlington
VA 22202-4302. Respondents should be aware that notwithstanding any other provision of law, no person shall be subject to a penalty for failing to comply with a collection of information if it

does not display a currently valid OMB control number.
1. REPORT DATE 3. DATES COVERED
OCT 1997 2. REPORT TYPE 00-00-1997 to 00-00-1997
4. TITLE AND SUBTITLE 5a. CONTRACT NUMBER
Asynchronous Data Fusion For AUV Navigation Via Heuristic Fuzzy
5b. GRANT NUMBER
Filtering Techniques
5c. PROGRAM ELEMENT NUMBER
6. AUTHOR(S) 5d. PROJECT NUMBER
5e. TASK NUMBER
5f. WORK UNIT NUMBER
7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES) 8. PERFORMING ORGANIZATION
Naval Postgraduate School,Department of Mechanical REPORT NUMBER
Engineering,Monterey,CA,93943
9. SPONSORING/MONITORING AGENCY NAME(S) AND ADDRESS(ES) 10. SPONSOR/MONITOR’S ACRONYM(S)
11. SPONSOR/MONITOR’S REPORT
NUMBER(S)
12. DISTRIBUTION/AVAILABILITY STATEMENT
Approved for public release; distribution unlimited
13. SUPPLEMENTARY NOTES
Proceedings IEEE, Oceans 97, Halifax, NS, Canada, 6-9 Oct 1997
14. ABSTRACT
This paper presents a heuristic fuzzy position estimation technique for autonomous un- derwater vehicle
navigation. The heuristic estimator performs asynchronous data fusion of all sensor measurements based
on their relative condence levels, and then nonlinearly combines the fused information with the INS
estimates via fuzzy ltering techniques. In this paper, the basis and implementation of the estimator will be
described, and naviga- tion results will be presented based on the heuristic estimator. In addition,
performance comparison based on the heuristic estimator and those based on extended Kalman lters will
be reported in our companion paper, and the results are expected to provide insights into the pros and cons
of individual methods in terms of computational cost, steady-state and convergence characteristics for bias
estimation.
15. SUBJECT TERMS
16. SECURITY CLASSIFICATION OF: 17. LIMITATION OF 18. NUMBER 19a. NAME OF
ABSTRACT OF PAGES RESPONSIBLE PERSON
a. REPORT b. ABSTRACT c. THIS PAGE Same as 6
unclassified unclassified unclassified Report (SAR)
Standard Form 298 (Rev. 8-98)
Prescribed by ANSI Std Z39-18
in water, thereby increasing the operational e(cid:14)ciency. where X is the arbiter output, Zi is the ith sensor
In terms of navigational capability, the OEX houses output, and Ci is its con(cid:12)dence value which has ac-
1) anacoustic doppler sensor which measuresaltitude countedforboththerangeerrorandbaselinegeometry
and vehicle velocity with respect to either water col- ofsatellites orsonarbeacons. Notethataconstraintof
N
umn or ground; 2) a Watson AHRS unit which mea- i=1Ci (cid:17)1 is imposed on the con(cid:12)dence values, and
sures Euler angles, tri-axial body rates and accelera- thus Ci can be loosely interpreted as prior probability
tion; and 3) a di(cid:11)erential GPS receiver unit; 4) RF tPhat the ith sensor is correct. Among these position
ethernet and 5) CTD sensor instrument which pro- sensorsonly the horizontal pro(cid:12)les aresought because
vides depth measurement. All sensors and actuators oftheir muchimprovedrangeerrortolerances andthe
onboardwereeachattachedtotheirindividual Neuron fact that the depth information can be obtained di-
Modules and the low-level control communication is rectly fromthedopplersonar. Nowtheremainingtask
done using the LonTalk protocol. High-level planning is to determine these Ci.
and control processes are carried out on the VxWorks
Oneimportantobjective inthispaperistostrive for
real-time operating system(with 68030CPU) andthe
a practical estimation algorithm which is not compu-
communicationamongprocessesisdata-drivenusinga
tational intensive but yet provides theoretically sound
sharedmemoryarchiecture with semaphoresandded-
approach in performing data fusion. To have a rea-
icated I/Ofunction calls (see [5,6,7]formoredetail).
sonable trade-o(cid:11), consider the following condition in
which the unknown quantity to be estimated is a con-
(cid:3)
stant, X , and is being observed by N independent
3 Heuristic Navigation Archi- sensors. It can be easily shown [1] that, in such a
case, an expression of the minimummean-squareder-
tecture ror estimate and the resulting error variance can be
established
Figure1showsageneralarchitecture forsettingupthe N;i6=j 2
(cid:5)j=1 ej
navigational modulein the OEX. In this architecture, Ci = (2)
therearearbiterscreatedforpositionsensors,attitude P
(cid:0)1
sensors and motion sensors. This set up is desirable N
(cid:3) 2 1
becausemanyexisting AUVsincorporatemultiplesen- C = E[(X (cid:0)X ) ]= 2 (3)
sors performing same functions, and it is thus bene(cid:12)- "i=1 ei#
cial to fuse all information to obtain the best naviga- X
ei = (cid:27)ihi (4)
tionestimates. Incasesofsensorfailure, thesearbiters
will recon(cid:12)gure in orderto complete time-critical mis- where ei is the expected position error standard de-
sions. viation for the ith sensor. (cid:27)i and hi represent the
range error and HDOP pro(cid:12)le for the ith sensor. (cid:5) is
Inputs to the position sensor arbiter are absolute
the product operator, and P corresponds to a sum of
AUV position measurements, which can be based on
N unique combinations of variance terms where each
(D)GPS and various forms of baseline sonar (cid:12)xes.
term consists of a product of N (cid:0)1 variances. C cor-
Thesemeasurementsareparticularly valuable because
responds to the overall error variance. For illustrative
of their drift-free properties over a longer time scale,
purposes,consider N =3,
as compared to the dead-reckoning position estimate.
However, over a shorter time scale, the DGPS (cid:12)xes e22e23
might be correlated thus introducing undesirable po- C1 = 2 2 2 2 2 2 (5)
sition error for compensation. It is thus important to e1e2+e1e3+e2e3
2 2
carefully sample these (cid:12)xes so that the signal-to-noise e1e3
ratio is maximized. In addition, measurements from C2 = 2 2 2 2 2 2 (6)
e1e2+e1e3+e2e3
allsensorsaregenerally unavailable ateverytimesam-
2 2
plinginstant, andastrategyisthusneededtocombine e1e2
C3 = 2 2 2 2 2 2 (7)
these asynchronous measurements before routing the e1e2+e1e3+e2e3
result to the heuristic position estimator. (cid:0)1
1 1 1
To combine the position measurements,each sensor C = 2 + 2 + 2 (8)
e1 e2 e3
is assigned a con(cid:12)dence value which characterizes its (cid:20) (cid:21)
expected variance in error about the true value. For Itshouldbenotedthatthesecon(cid:12)dencevaluesareonly
an example, typical DGPS (cid:12)xes can have 1-5m range meaningful if the statistical properties of the sensor
error ((cid:27)) together with horizontal dillution of preci- measurements are stationary, and the state dynamics
sion (HDOP) uncertainty due to satellite geometry. arenegligible althoughthee(cid:11)ectofstatedynamicswill
The total position error introduced in terms of root- be studied and reported in our companion paper.
mean-squarevalue is HDOP (cid:2)(cid:27). It should be noted
that while the range error is generally hard to quan- Similarly, the attitude sensor arbiter generates the
tify, the HDOP pro(cid:12)le can be readily obtained from bestestimateoftheEuleranglesandbody-(cid:12)xedangu-
any receiver, and thus it should be accounted for in lar rates. This arbiter setting enables direct compari-
the position estimator otherwise position error might soninperformancesusingdi(cid:11)erent compasseswithout
becompromised. Once asuitable timesamplinginter- requiring any code modi(cid:12)cation. The arbiter outputs
val is chosen, the output of the position sensor arbiter are then routed to both the heuristic position estima-
is given as tor and motion sensor arbiter.
In the motion sensor arbiter, the doppler-based
X = Zi(cid:1)Ci (1) ground and water velocities are compensated based
X
2
0-7803-4111-2/97 $10.00 (c) 1997 IEEE
on the CTD output. At present, the default sound de(cid:12)ned based on the assumption of gaussian sensor
speed was set to 1500m/s and correction was made noise distribution. It should be noted that in a com-
in post-processing. Typical variation in sound speed panion paper, the same data sets were (cid:12)ltered using
1
with respect to the default setting can be as high as extended 2D Kalman (cid:12)lters , and the objective for
2%, and this error must be accounted for during long such arrangement is to compare the performance be-
underwater transits. tween usingnumerically intensive algorithmswithbias
estimation and the heuristic algorithms without bias
estimationbutrelying onpositionmeasurements. The
critical factorsto beconsidered forthe comparisonin-
3.1 Heuristic Position Estimator
clude the convergence rate and steady state error in
(HPE)
the bias estimates and magnitude of drift induced be-
tween (cid:12)xes. Among these data sets, only DGPS po-
Figure 2 shows a block diagram of the heuristic es-
sition (cid:12)xes were available, and thus the true heading
timator which considers only the ground speed as a
reference can only be deduced when the OEX was on
special case in order to simplify explanation. Xn and surface. It should be noted that, to accomodateasyn-
Xe correspondto the absolute position measurements chronous data fusion, the position estimator integra-
de(cid:12)ned in north-east co-ordinates given a pre-de(cid:12)ned
tion time step was chosen to be the smallest inter-
origin. Similarly, X^n and X^e correspond to the posi- val with respect to all sensor throughputs(amongthe
tion estimator outputs using the same origin. In the results presented, the sampling rate was set to 8Hz,
diagram,the outputsofthe attitude sensorarbiter are (cid:27)dgps =2 and (cid:27)gps = 30. In cases when there is no
used to transform the ground speed from the body- position (cid:12)x(es) available duringsometime interval, no
(cid:12)xed frame to geographical frame. If there is no po- o(cid:11)setswill beaddedtothepositionestimatoroutputs.
sition measurement,the position estimator is then re-
ducedtoadead-reckoner,andnoo(cid:11)setsareneededfor
X^nandX^e. Ifthereexistsatleastonepositionupdate,
the position residual error (the di(cid:11)erence between the 4 Results
measurementandheuristic position estimator output)
is then nonlinearly gain scheduled in order to gener-
Navigation results presented in this paper were based
ate appropriate o(cid:11)sets for X^n and X^e for each of the on a combination of the Motorola GPS receiver, Acu-
absolute position measurement,and the nonlinear Ke point di(cid:11)erential receiver and the doppler-aided INS
and Kn are given as system. To provide important insights, the GPS per-
formance was (cid:12)rst characterized in a controlled envi-
(cid:14)n = Xn(cid:0)X^n (9) ronment, followed by the evaluation of the heuristic
(cid:14)e = Xe(cid:0)X^e (10) position estimation performance which was based on
at-sea data.
X^n+(k) = X^n(cid:0)(k)+Kn(cid:14)n (11)
Figure 4 and 5 show respectively the time histories
X^e+(k) = X^e(cid:0)(k)+Ke(cid:14)e (12) of (cid:12)xes and the converted X-Y excursion (at a lat-
2 2
(cid:0)(cid:14)e=C itude of 26 degrees, Boca Raton, Florida) collected
Ke((cid:14)e) = 1(cid:0)exp (13) in a single experiment during which the OEX was lo-
2 2
Kn((cid:14)n) = 1(cid:0)exp(cid:0)(cid:14)n=C (14) catedinacircularsalt-waterpoolof10ftdiameter,and
the GPS and FM antennas were held above the sur-
Fromtheseexpressions,itcanbeeasilyseenthatwhen face. It should benoted that the OEX was not rigidly
there is small residual error with respect to C, the mounted thus slight vehicle movement should be ex-
HPEdiscountsthecontributionfromthemeasurement pected although it was constrained to within 2 ft in
as primarily sensor uncertainty, and resorts to dead- radius,which is well within the rated DGPSaccuracy.
reckoning which generally provides a smoother posi- The choice of pool test was made so as to minimize
tion estimate. In the case the residual error becomes any e(cid:11)ect associated with the controlled environment.
relatively signi(cid:12)cant, the HPE assumes that the er-
Time history plots consist of longitude (meter), lat-
ror is largely caused by the motion sensor drift, and
itude (meter) and the GPS status (0: NO GPS, 1:
thus weighs heavily on the measurement. Figure 3
GPS, 2: DGPS). It can be seen that the distribution
provides a (cid:13)ow chart of the detailed operation at ev-
of GPS and DGPS (cid:12)xes were fairly uniform,and dur-
erynavigation cycle. Byexaminingthe(cid:13)owchart,one
ing the time when DGPS (cid:12)xes were available (around
can see that when both water and ground speed are
2500sec), there was minimal (cid:13)uctuation in both lati-
available, currents will be estimated in the geograph-
tude and longitude. On the contrary, when only the
ical frame after transforming the speed di(cid:11)erence. In
GPS (cid:12)xes were available (around 250sec), there was
cases where there is a loss of bottomtrack at large al-
signi(cid:12)cant (cid:13)uctuation observed, and the apparent ve-
titude, the current estimate canbeusedtogether with
locity speed was as high as 0.1 knot, as indicated by
the water speed to estimate the ground speed over a
the time histories data. It is thus important to con-
relatively short time scale within which the estimated
sider the e(cid:11)ect of GPS correlated noise and sampling
currentsmagnitudeisslowlydecaying. Twoimportant
rate when estimating currents over a short temporal
observations can be made regarding the HPE perfor-
scale. Nevertheless, the navigation performance can
mance. Firstly, the HPE does not explicitly estimate
be also critically dependent on the speci(cid:12)c trial loca-
the sensorbiases andthusthe navigation performance
without any position measurements is no better than 1
It is worth mentioning that standard Kalman (cid:12)lters can-
that of standard dead-reckoning estimator. Secondly, not bedirectledappliedto navigation problemsbecauseof the
the nonlinear error correcting gains are heuristically kinematicstransformbetweenco-ordinateframes.
3
0-7803-4111-2/97 $10.00 (c) 1997 IEEE
tion, and in particular, the e(cid:11)ect of GPS noise will navigation results which were based on this estima-
becomerelatively insigni(cid:12)cant when the OEX is to be tor has been illustrated. This paper was also empha-
operated in Florida currents of 3-5 knots, and in deep sized that the navigation performance must be eval-
water regions with only water speed measurements. uated with respect to the entire system as a whole,
rather than an isolated entity. Thus, factors such as
FromtheXYplot,itcanbeseenthatall errorswere
mission requirements, systems recon(cid:12)gurability, cost-
con(cid:12)ned to within a 5 meter square area. To evaluate
e(cid:11)ectiveness and computing resources must be alto-
the worst case sampling interval, one can consider the
gether considered with regard to the constrainted re-
following ideal condition underwhich the OEXcruises
sources currently available on the OEX. Results of
horizontally along for T seconds at u knots with (cid:14)u
such evaluation can then provide important insights
speed error and (cid:14) heading error. The position error,
into the e(cid:11)ectiveness ofsensor-basedandmodel-based
(cid:14)X, can be crudely approximated as
approach. For long underwater missions without any
2 2 2 sonar positional beacons and with minimal surfacing
(cid:14)X =T u (cid:14) +(cid:14)u (15)
frequency, estimation of sensor biases might be re-
quired for high precision navigation, and such infor-
For an exampple, given DGPS (cid:12)xes with 5 meter er-
mation must be extracted. Extended Kalman (cid:12)lters
ror, the OEX cruises at 2 knots with 1% speed error
were applied to those data sets with known heading
and2degree heading error,T is approximately 3min-
bias, and the results will be reported in our compan-
utes. That means, the maximumsampling interval is
ion paper in terms of steady-state error and conver-
3 minutes or position estimation performance will be
gence performance. Other than focusing on the posi-
signi(cid:12)cantly compromised. On the contrary, T should
tion sensormeasurements,parallel work on improving
not be much smaller than 3 minutes otherwise the es-
our existing INS system in currently underway such
timator will be signi(cid:12)cantly biased with GPS noise.
as localizing the main source of interference, demag-
Figure 6 and 7 compare the INS way-point naviga- netizing the battery housing, and developing a more
tion performance based on 1) doppler returns (dash accurate deviation table based on a (cid:12)ber-optic gyro
line) and 2) GPS + doppler (solid line). In these (Andrews Corp) as short-termheading reference.
(cid:12)gures, ‘X’ and ‘O’ represent a di(cid:11)erential and reg-
ular GPS (cid:12)x respectively, and in both missions the
OEXwasstartedattheorigin. Duringthese missions,
References
the OEX was underwater most of the time, and com-
mandedto surface during somespeci(cid:12)ed cornerings in
order to obtain (cid:12)xes, and thus position drift due to [1] A. Gelb, Applied Optimal Estimation, ed. A.
doppler and attitude sensors can be easily observed, Gelb, MIT Press, 1986.
as compared to the DGPS (cid:12)xes as the only source of
[2] G. Lachapelle, M.E. Cannon,G. Lu, B. Loncare-
reference. At the end of the last eastward leg in Fig-
vic Shipborne GPS Attitude Determination Dur-
ure 6, there was a signi(cid:12)cant discrepancy between the
ing MMST-93, IEEE Journal of Oceanic Engi-
position estimator and DGPS measurement(approxi-
neering, Vol.21, No.1, pp.100-105,January, 1996.
mately 1.5% error based on 50 meters after 3300 me-
ters transect). It is worthmentioning thatduring pre- [3] R.B. McGhee, J.R. Clynch, A.J. Healey, S.H.
vious OEX surface trials with continuous DGPS (cid:12)xes Kwak, D.P. Brutzman, X.P. Yun, N.A. Norton,
(notincluded in this paper),thedopplermeasurement R.H. Whalen, E.R. Bachmann, D.L. Gay, W.R.
error was found to lie within 1%, and the additional Schubert An ExperimentalStudy of an Integrated
error herein was believed to be caused mainly by the GPS/INS System for Shallow-Water AUV Navi-
heading measurementwith internal magnetic interfer- gation(SANS),ProceedingofUnmannedUnteth-
ence. Figure 7 presents results of a 3-hr mission cov- ered Submersible Technology Symposium, 15p,
ering 15kmtransect. Sporadic(cid:12)xes can be seen in the Durham,New Hampshire, September, 1995.
(cid:12)gurewhichcorrespondstotheOEXsurfacingmaneu-
vers. Among these (cid:12)xes, maximum discrepancy be- [4] M.J. Rendas, I.M.G. Lourtie Hybrid Navigation
tween the position estimator andGPS (cid:12)xes wasfound System for Long Range Operation, AUV 94,
at location [-50 east, 150 north], and also it can be pp.353-359,1994.
seen that the position estimatorrespondsless to these
GPS(cid:12)xes,butmuchmoretotheDGPS(cid:12)xesobtained [5] S.M. Smith, S.E. Dunn, T.L. Hopkins, K. Heeb,
immediately afterwards. By observation, the discrep- T. Pantelakis The Applications of a Modular
ancy was approximately 100m since the last update AUV to Coastal Oceanography: Case Study on
(6 legs of transect away (cid:25) 3300m),and thus the error the Ocean Explorer, IEEEOceans 95Conference,
was approximately 3%. It should be noted that 100m pp.1423-1432,San Diego, CA, October, 1995.
is within the limit ofthe GPSerrordeviation, andthe
[6] S.M.Smith,K.Ganesan,T.Flanigan,L.Marquis
result suggests that accurate navigation does not nec-
TheIntelligentDistributedControl SystemArchi-
essarily require frequent surfacing, as expected from
tecture in the Ocean Voyager II and Ocean Ex-
previous discussion.
plorer Vehicle, 9th International Symposium on
Untethered SubmersibleTechnology, pp.395-405,
Durham,NH, September, 1995.
5 Remarks
[7] S.M. Smith, S.E. Dunn, P.E. An Data Collec-
tion with Multiple AUVs for Coastal Oceanog-
In this paper, the basis and implementation of the raphy, Oceanology International 96, pp.263-279,
heuristic position estimator has been described, and Brighton, U.K, March, 1996.
4
0-7803-4111-2/97 $10.00 (c) 1997 IEEE
[8] K.P.Schwarz, M.Wei, M.V.Gelderen Aided Ver-
sus Embedded: AComparison ofTwoApproaches
to GPS/INS Integration, pp. 314-322,1994. if status == run
was origin updated? N are Nav sensors Y set sensor flag to TRUE
updated? save last update time
Y
set position to origin
GPS LBL SBL Precision Nav Watson DVL CTD
N
newsonic vs. DVL water
. . . Y
new DVL water newSonic?
Y
Position Sensor Attitude Sensor Sound Speed get new DVL water N ctoom apcuctoeu hnot rfoizro dnetaplt hw achtearn vgeeloscity
Arbiter Arbiter Compensator save last depth and update time
combine DVL and sonic
water speed estimates
via center of mass
water ground
beys tliamsat tceu grrreonutn eds stipmeaetde N new DVL ground? Y compute ground and
absolute Motion Sensor and water speed current velocity
position
measurement
Arbiter compute dead-reckoned
position estimate
Heuristic new GPS or new DGPS N position estimate =
Position Estimator or LBL? dead reckoned
Y
combine GPS and LBL convert position estimate
to nav and ECEF frames
compute dead-reckoned write position estimate
position offset back to shared memory
position currents
estimates estimates position estimate = return
offseted DR estimate
Figure 3: A (cid:13)ow chart of the position estimator algo-
Figure 1: High-level arbiters for setting up heuristic
rithm.
position estimator.
u,v,w attitude
Ke(d e) d n Absolute
Body-Nav Position
Transformation Measurement DGPS Fixes in Pool: 13:44, 8/6/96
s) 5
er
Vn X_n X_n North e (met 0
d
u
git
n
o
L−5
0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
Ve X_e X_e East 10
s)
d er
Kn(d n) e met 5
e (
d 0
u
atit
l−5
Figure 2: Closed-loop heuristic position estimator. 0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
3
s2
u
at
St1
0
0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000
Time (seconds)
Figure4: MotorolaGPStime-historyerrorevaluation.
5
0-7803-4111-2/97 $10.00 (c) 1997 IEEE
DGPS Fixes (Pool Test): 13:44pm, 8/6/1996
4.5
4
3.5
3
ers)2.5
et
m
Y (in 2
1.5
1
0.5 042597:1131 (cyan: AUV−PV)
500
0
−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5
X (in meters)
400
Figure 5: Motorola GPS X-Y error evaluation.
300
ers)
et
m 200
h (
ort
N
100
032597:1259 (red 0: gps; blue x: dgps)
300
0
250
200 −100
−200 −100 0 100 200 300 400 500
East (meters)
150
s)
er Figure 7: Heuristic position estimator and GPS out-
met 100 puts. Solid line: heuristic position estimator output,
h ( anddashline: dead-reckoned output. ’X’and’O’rep-
ort resent di(cid:11)erential and regular GPS (cid:12)x respectively.
N
50
0
−50
−100
−100 0 100 200 300 400 500 600
East (meters)
Figure 6: Heuristic position estimator and GPS out-
puts. Solid line: heuristic position estimator output,
anddashline: dead-reckoned output. ’X’and’O’rep-
resent di(cid:11)erential and regular GPS (cid:12)x respectively.
6
0-7803-4111-2/97 $10.00 (c) 1997 IEEE