Terrain Navigation using Bayesian Statistics

Niclas Bergman, LennartLjung, and Fredrik Gustafsson

Department of Electrical Engineering

Linkoping University, S-581 83Linkoping, Sweden

WWW: http://www.control.isy.liu .se

Email: ljung@isy.liu.se, fredrik@isy.liu.se

1999-03-11

REG

LERTEKNIK

AUTO

_{MATIC CONTR}

OL

LINKÖPING

Report no.: LiTH-ISY-R-2139

For the IEEE Control Systems Magazine

TechnicalreportsfromtheAutomaticControlgroupinLinkopingareavailable

by anonymous ftp at the address ftp.control.isy.liu.se. This report is

Dept. of Electrical Engineering, Linkoping University, Sweden

E-mail: fniclas,ljung,fredrikg@isy. liu. se

URL:http://www.control.isy.l iu.s e

Inaircraftnavigationthedemandsonreliabilityandsafetyareveryhigh. The

importanceofaccuratepositionandvelocityinformationbecomescrucialwhen

yinganaircraftatlowaltitudes,andespeciallyduringthelandingphase. Not

onlyshould thenavigationsystemhaveaconsistentdescriptionoftheposition

oftheaircraft,but alsoadescriptionofthesurrounding terrain,buildingsand

otherobjectsthat areclose to theaircraft. Terrainnavigation isanavigation

schemethatutilizesvariationsintheterrainheightalongtheaircraft ightpath.

IntegratedwithanInertialNavigationSystem(INS),ityieldshighperformance

informa-recursiveestimationproblemmustbesolvedon-line. Traditionally,thisltering

problem hasbeensolvedby local linearizationof theterrain at one orseveral

assumed aircraftpositions. Due to changingterrain characteristics,these

lin-earizations will in some cases result in diverging position estimates. In this

work, we show how the Bayesian approach gives acomprehensive framework

for solvingthe recursiveestimation problem in terrain navigation. Instead of

approximatingthemodelof the estimationproblem, the analyticalsolutionis

approximatelyimplemented. Theproposed navigationltercomputesa

prob-ability mass distribution of the aircraft position and updates this description

recursivelywitheachnewmeasurement. Thenavigationlterisevaluatedover

acommercialterraindatabase,yieldingaccuratepositionestimatesoverseveral

typesofterrain characteristics. Moreover,in aMonte Carloanalysis,itshows

optimalperformanceasitreachestheCramer-Raolowerbound.

1 Aircraft Navigation

Navigationis theconceptof determination ofthekinematicstateof amoving

vehicle. Inaircraft navigationthis usually consistsof ndingthe position and

velocity of the aircraft. Accurate knowledge of this state is critical for ight

safety. Therefore, an aircraftnavigationsystem should notonly providea

re-liableandaccurate estimateofthe currentkinematicstateoftheaircraft, but

alsoaconsistentdescriptionoftheaccuracyofthisestimate.

Aircraft navigation is typically performed using a combination of

dead-reckoningandxpositionupdates. Indead-reckoningsystems,thestatevector

is calculated from a continuous series of measurements of the aircraft

move-mentrelativetoaninitialposition. Duetoerroraccumulation,dead-reckoning

systemsmust bere-initialized periodically. Fix point, or positioning, systems

measurethestatevectormoreorlesswithoutregardtothepreviousmovement

oftheaircraft. Theyarethereforesuitableforre-initializationofdead-reckoning

systems.

Themostcommondead-reckoningsystemsaretheInertialNavigation

inaknownorientationwithrespecttoaxed,non-rotatingcoordinatesystem,

commonlyreferredtoasinertialspace,ormeasurestheangularrateofthe

ac-celerometersrelative to inertial space. The inertial navigation computeruses

thesesensedaccelerationsandangularratestocomputetheaircraftvelocity,

po-sition,attitude,attituderate,heading,altitude,andpossiblyrangeandbearing

to destination. An INS generates near instantaneous continuousposition and

velocity,itisself-contained,functionsatalllatitudes,andinallweather

condi-tions. Itoperatesindependentlyofaircraftmanoeuvresandwithouttheneedfor

groundstationsupport. Completeand comprehensivepresentationsof inertial

navigationcanbefoundin [14,16].

Positioningsystemsthathaveattractedalotofattentionlatelyaretheglobal

satellitenavigationsystemswhichpromiseaveryhighaccuracyandglobal

cov-erage. Therearetwoglobalsatellitesystemsfornavigationin usetoday: GPS,

developedbytheU.S.,andtheRussiansystemGLONASS. Positionestimates

areobtainedbycomparingdistancesfromtheaircrafttofourormoresatellites.

Thesystemshavebeendevelopedformilitarypurposesandseveralcoding

tech-niquesareusedtokeeptheaccuracyforcivilianorunauthorizedusersatalevel

far from the actual performance of the systems. However, using ground

sta-tionsas reference, the coding errorscan be removedeÆciently. Vendors have

o-the-shelfreceiversfordierentialGPS(DGPS)withapositionaccuracy

be-lowtheonemeter level. A comprehensivesummary ofthe conceptofsatellite

navigationcanbefoundin [14,Chapter5].

Theradio navigationsystemshavethedisadvantageofrelyingon

informa-tionbroadcastedtotheaircraft. Thisinformationcouldbedeliberatelyjammed

inahostilesituation,orthetransmitterscouldbedestroyed,leavingtheaircraft

withoutnavigationsupport. Hence, evenif thesatellite systemsgivehigh

ac-curacypositioninformation theyneedto becombinedwithalternativebackup

systems using other navigation principles. The concept of terrain navigation

isanalternativepositioningtechniquethatautonomouslygeneratesupdatesto

theINS,althoughingeneralnotwiththesameaccuracyasthesatellitesystems.

Themainideain terrainnavigationis tomeasure thevariationsin theterrain

Altitude Groundclearance

Meansea-level

Terrainelevation

Figure1: Theprincipleofterrainnavigation.

ure1. Theaircraftaltitudeovermean sea-levelismeasuredwithabarometric

altimeterand thegroundclearance ismeasuredwith aradaraltimeter,

point-ing downward. The terrain elevation beneath the aircraft is found by taking

thedierence between the altitudeand ground clearance measurements. The

navigation computer holds a digital reference map with values of the terrain

elevation as afunction oflongitude andlatitude. The measuredterrain

eleva-tioniscomparedwiththisreferencemapandmatchingpositionsinthemapare

determined. Terrainrepetitiveness and atness makethis matching nontrivial

andthe qualityof theoutcome dependent ontheamountof terrainvariation.

Many areas inside the reference map will in general have a terrain elevation

comparabletothemeasuredone. Inordertodistinguishthetruepositionfrom

falseones,severalmeasurementsalongtheaircraft ightpathneedto be

con-sidered. Hence,themeasurementsmust bematched withthemap on-lineand

inarecursivemanner. Forcomprehensivediscussionsabouttheapplicationsof

terrainnavigationtechniques,see[10,11].

Theperformanceofthematchingofterrainelevationmeasurementswiththe

mapdependshighlyonthetypeofterraininthearea. Flatterraingiveslittleor

betweenseveral,wellmatchingtracks. Theinformationcontentinsideageneric

area of the map can be shown to be proportional to the average size of the

terraingradient, v u u t 1 N N X i=1 krh(x i )k 2 (1) wherex i

arepositionsuniformlydistributed intheareaofinterest. Thisscalar

measureoftheterrain informationcanbeconnectedto anassociatedCram

er-Raoboundfortheunderlyingestimationproblem[3]. TherightpartofFigure2

shows(1)evaluatedinsquareblocksof400meter sidewherebrightcolor

indi-catesalargevalue. Theterrainmapusedinthisworkisarealcommercialmap

Figure2: Theleftpartshowstheterrainheightandtherightpartthe

informa-tioncontentinthemapoveracentralpartofSweden.

ofa100by100km areaofcentral Sweden. Thepureterrainelevationsamples

aregivenin auniformmeshof50by50meterresolutionandshowntotheleft

inthegure. Usinginterpolationfromsurroundingmapvalues,theterrainmap

canberegardedas aknownlook-uptable ofterrainelevationsasafunctionof

position. Everything that in the real world cannot be found by interpolation

fromthedatabasevaluesmustberegardedasnoise. TherightpartofFigure2

navigationinformation, while the southernpartconsists ofveryroughterrain

withvarying hillsofsomehundredmetersheightandnarrowvalleys thatgive

ahigh informationcontent. Maps such asthe rightpart ofFigure 2could be

usedfor mission planningpurposes, e.g.,ndingthemost informativepath to

thenal destination.

There are several commercial algorithms that solve the terrain navigation

problem. Since the development has been driven by military interests, most

of these are not verywell documented in the literature. The most frequently

referredalgorithmsforterrainnavigationareTERCOM(terraincontour

match-ing)and SITAN(Sandiainertialterrain-aidednavigation). TERCOMisbatch

orientedandcorrelatesgatheredterrainelevationproleswiththemap

period-ically[2,9,16]. Theaircraftisnotallowedtomaneuverduringdataacquisition

inTERCOMandthereforeithasmainlybeenusedforautonomouscrafts,like

cruisemissiles. SITAN isrecursiveandusesamodiedversionof anextended

Kalmanlter(EKF)initsoriginalformulation[13]. When yingoverfairly at

oroververyroughterrain,orwhentheaircraftishighlymaneuverable,this

al-gorithmdoesnotingeneralperformwell. Inordertoovercomethesedivergence

problemsparallelEKFshavebeenusedin[6,12]. Anotherwidespreadsystemis

theTERPROMsystem,developedbyBritishAerospace,canbefoundinseveral

NATOaircraft. Itisahybridsolution,inwhichanacquisition-modecorrelates

measurementsin batch to ndan initialposition andin track-mode processes

measurementsrecursivelyusingKalmanltertechniques. However,dueto

com-mercialinterestsandbecauseofitsuseasaclassiedmilitarysystem,itisnot

aswelldocumentedintheliteratureastheprevioustwo. Onemorerecentand

dierentapproachthattriestodealwiththenonlinearproblemsisVATAN[8].

InVATAN theViterbialgorithm isapplied to theterrainnavigationproblem,

yieldingamaximumaposterioripositionestimate.

Inthiswork,wetakeacompletelystatisticalviewontheproblemandsolve

thematching with themap as arecursivenonlinearestimation problem. The

conceptual solution is described in the following section and an approximate

implementation in Section 3. Simulation resultswith this implementation are

AsshowninFigure1thedierencebetweenthealtitudeestimateandthe

mea-suredgroundclearanceyieldsameasurementoftheterrainelevation. Assuming

additivemeasurementnoisetheterrainelevationy t

relatestothecurrentaircraft

position x t accordingto y t =h(x t )+e t (2)

wherethefunctionh(
):R 2

7!Ristheterrainelevationmap. Themeasurement

noisee t

isawhiteprocesswithsomeknowndistribution p et

(
). This

measure-ment error models both the errors in the radar altimeter measurements, the

current altitude estimate and errors originating from the interpolation in the

terrainmapnotperfectlyresemblingtherealworld. Letu t

denotetheestimate

of the relative movement of the aircraft between twomeasurements obtained

from the INS. Modeling the dead-reckoningdrift of the INS witha white

ad-ditiveprocess v t

, the absolute movement of the aircraftobeys asimple linear

relation x t+1 =x t +u t +v t (3) where v t

is distributed accordingto some assumed known probability density

functionp vt

(
). Summarizingequations(2) and(3)yieldsthenonlinearmodel

x t+1 =x t +u t +v t y t =h(x t )+e t t=0;1::: (4) wherev t ande t

aremutuallyindependentwhiteprocesses,bothofthem

uncor-relatedwith theinitial state x 0

which is distributed accordingto p(x 0

). One

mayarguethattheINSestimateu t

shouldberegardedasasensormeasurement

insteadofas aknownparameterinthestatetransitionequation,

u t =x t+1 x t +e INS t :

Thiswouldrequiretheintroductionofanewstatevectorincorporatingbothx t

and x t+1

in orderto retainthe Markovianpropertyof the statespace model.

Instead, we choose to include the error e INS t

in the process noise v t

requiredtorecursivelycomputeanapproximationoftheconditional densityof

thestates.

Theobjectiveoftheterrainnavigationalgorithmisto estimatethecurrent

aircraftpositionx t

usingtheobservationscollecteduntilpresenttime

Y t =fy i g t i=0 :

WithaBayesianapproachtorecursiveltering,everythingworthknowingabout

thestateattimetiscondensedintheconditionaldensityp(x t

jY t

). Withsome

abuseofnotation, thedistribution of agenericrandom variablez conditioned

onanotherrelatedrandomvariablewis

p(zjw)= p(z;w) p(w) = p(wjz)p(z) p(w) (5) Assumethatp(x t jY t 1

)isknownandapply(5)tothelastmemberintheset

Y t , p(x t jY t )= p(y t jx t ;Y t 1 )p(x t jY t 1 ) p(y t jY t 1 ) :

Insertingthemodel(4)andnotingthatthedenominator isascalar

normaliza-tionconstantyield,

p(x t jY t )= 1 t p et (y t h(x t ))p(x t jY t 1 )  t = Z p et (y t h(x t ))p(x t jY t 1 )dx t

which describesthe in uence ofthe measurement. Using (5) thejoint density

ofthestatesattwomeasurementinstantsis

p(x t+1 ;x t )=p(x t+1 jx t )p(x t ):

Thedensityupdate betweentwomeasurementsis foundby marginalizingthis

expressiononthestatex t andinserting(4) , p(x t+1 jY t )= Z p v t (x t+1 x t u t )p(x t jY t )dx t :

Thiscompletesoneiterationoftherecursivesolution. Summarizingthe

0 1 0 p(x t jY t )= 1 t p et (y t h(x t ))p(x t jY t 1 ) p(x t+1 jY t )= Z p vt (x t+1 x t u t )p(x t jY t )dx t (6) where  t = Z p et (y t h(x t ))p(x t jY t 1 )dx t :

The Bayesiansolution is a density function describing the distribution of the

statesgiventhecollectedmeasurements. Fromtheconditionaldensity, apoint

estimatesuchastheminimummeansquareerrorestimatecanbeformed

^ x t = Z x t p(x t jY t )dx t : (7)

Assumingthat thisestimateisunbiased,thecovariance

C t = Z (x t ^ x t )(x t ^ x t ) T p(x t jY t )dx t (8)

quantizes theaccuracyof theestimate. Equation(8)is convenientwhen

com-paring(7)withestimatesfrom othernavigationsystems.

Therecursiveupdateoftheconditionaldensity(6) describeshowthe

mea-surementy t

andtherelativemovementu t

aecttheknowledgeabouttheaircraft

position. With eachnewterrain elevation measurement, thepriordistribution

p(x t

jY t 1

)ismultiplicativelyampliedbythelikelihood ofthemeasurement

y t

. Thismeansthat the conditionalprobabilitywill decreasein unlikelyareas

andincreaseinareaswhereitislikelythatthemeasurementwasobtained.

Be-tweentwomeasurements,thedensityfunctionp(x t

jY t

)istranslatedaccording

tothe relative movementof theaircraft obtainedfrom the INSand convolved

withthe densityfunction ofthe errorof this estimate. Thusthe support and

shapeoftheconditionaldensitywilladapttoareaswhichtthemeasurements

wellandfollowthemovementobtainedfromtheINS.Itisworthnotingthat(6)

istheBayesiansolutionto (4)for allpossiblenonlinearfunctions h(
) andfor

anynoisedistributions p v

t

(
)and p e

t

(
). Inthespecialcase oflinear

measure-ment equation and Gaussian distributed noises the equations above coincide

ing several integrals. Due to the unstructured nonlinearity h(
), these

inte-grationsare in general impossible to solvein closed form and therefore there

existsnosolutionthatupdatestheconditionaldensityanalytically. The

imple-mentationmustthereforeinevitablybeapproximate. Astraightforwardwayto

implement thesolutionis tosimply evaluatetherecursion in severalpositions

inside the area where the aircraft is assumed to be and update these values

furtherthroughtherecursion.With suchaquantizationofthestatespace,the

integralsin (6) turn into sums overthe chosenpoint values. The earliest

ref-erenceofsuchanumericalapproachto solvingthenonlinearlteringproblem

is[7]. Morerecentreferencesinvolvethep-vectorapproachin[17]andaslightly

dierentapproach,presentedin [15],usingapiecewiseconstantapproximation

tothedensityfunction. Intheterrainnavigationproblem thestatedimension

istwoandthequantizationcaningeneralbeviewedasabed-of-nailswherethe

lengthof each nail corresponds to acertainelementary massin that position.

Theimplementationdescribedinthispaperisthereforelabeledthepoint-mass

lter(PMF).

3 The Point-Mass Filter

Assume that N grid points in R 2

have been chosenfor the approximationof

p(x t

jY t

). Introducethenotation

x t

(k) k=1;2;:::;N

for these N vectors in R 2

. Each of these N grid points has a corresponding

probabilitymass p( x t (k)jY t ) k=1;2;:::;N:

Inorder to obtainasimpleand eÆcientalgorithm, thegridpointsarechosen

fromauniformrectangularmeshwithresolutionofÆmetersbetweeneachgrid

Z R 2 f(x t )dx t  N X k =1 f(x t (k))Æ 2 :

Applyingthisapproximationto (6)yieldstheBayesianpoint-massrecursion:

p(x t (k)jY t )= 1 t p e t ( y t h(x t (k)))p(x t (k)jY t 1 ) x t+1 (k)=x t (k)+u t k=1;2;:::;N p(x t+1 (k)jY t )= N X n=1 p v t ( x t+1 (k) x t (n))p(x t (n)jY t )Æ 2 (9) where  t = N X k =1 p et (y t h(x t (k)))p( x t (k)jY t 1 )Æ 2 : (10)

Thetime updatehasbeensplitintotwoparts. Firstthegridpointsare

trans-latedwiththeINSrelativemovementestimateu t

andthentheprobabilitymass

densityisconvolvedwiththedensityp v

t

(
). Thepointestimate(7)iscomputed

ateach iterationasthecenterofmassof thepoint-massdensity,

^ x t = N X k =1 x t (k)p( x t (k)jY t )Æ 2 :

Hence,theestimatedoesnotnecessarilyfallonagridpoint.

Inorder to follow theaircraftmovementsthe gridmust be adapted to the

supportoftheconditionaldensity. Aftereachmeasurementupdate, everygrid

pointwithaweightlessthan">0timestheaveragemassvalue

1 N N X k =1 p(x t (k)jY t )= 1 NÆ 2 :

isremovedfromthegrid. Thenewset ofgridpointsisdened by

 x t (k):p(x t (k)jY t )>"=NÆ 2 :

Theweightsneedtobere-normalizedafterthistruncationoperation. The

trun-cationwillmakethealgorithmfocusonareaswithhighprobabilityandremove

gridpointsin areaswhere theconditionaldensityissmall. Thebasicgrid

The prior will then have a large support, and naturally it is not interesting

to have a high grid resolution. Instead we start with a sparse grid and run

the algorithm and removeweightsusing thetruncation operationabove until

thenumberofremaining gridpointsfalls belowsomethresholdN 0

. Then the

mesh resolution canbe increasedand the algorithm continuedto processnew

measurements, updating the conditional density in the new dense grid. The

up-samplingis performedbyplacingonegridpointbetweeneveryneighboring

gridpointin themeshusing linearinterpolationto determineitsweight. This

willyield adoublingofthe meshresolution. Theconvolutionin (9) will

intro-ducesomeextragridpointsalongtheborderofthepoint-massapproximation,

increasingthesupportofthemesh. Ifthemeasurementshavelowinformation

contenttherewillbeanet-increaseofgridpointseventhoughsomeareremoved

bythetruncation operation. Therefore, ifthenumberof gridpointsincreases

abovesomethresholdN 1

,themeshisdecimatedbyremovingeverysecondgrid

pointfromthemesh,halvingthemeshresolution.

Hence,thenumberofgridpoints N, thepoint-masssupportand themesh

resolution Æ isautomatically adjusted through each iteration of thealgorithm

usingthedesignparameters",N 0

andN

1

. Anillustrationisgivenin Figure6.

InsummarythePMFalgorithmconsistsof (9)andtheappropriateresampling

of the grid described above. Details about the implementation and the grid

renementprocedure canbefoundin [3].

The renement of the grid support and resolution in the PMF described

above is of course ad hoc, and one may wonder if this actually works. The

Cramer-Raolower bound is a fundamental limit on the achievable algorithm

performance which can be used to evaluate the average performance of the

PMFand verify thattheltersolvesthenonlinearlteringproblem withnear

optimalperformance. Theresultsarepresentedherewithoutproofsor

deriva-tions, see [3{5] for details. Let N(m;P) denote the n-dimensional Gaussian

distributionwithmeanvectorandcovariancematrixP

N(;P)= 1 p (2) n jPj exp 1 2 (x ) T P 1 (x )
:

p e t (
)=N(0;R t ) p v t (
)=N(0;Q t ) p(x 0 )=N(^x 0 ;P 0 );

the Cramer-Raolower bound for the one step ahead prediction of the states

satisesthematrix(Riccati) recursion,

P t+1 =P t P t H t (H T t P t H t +R t ) 1 H T t P t +Q t initiatedwithP 0 . AboveH t

isthe gradientofh(
) evaluated at thetruestate

valueat timet, H t =rh(x t ):

Thus, theCramer-Raoboundisafunction ofthenoiselevelsandthegradient

oftheterrain alongthetruestatesequence.

TheCramer-Raoboundsetsalowerlimitontheestimationerrorcovariance

whichdependsonthestatisticalpropertiesofthemodel(4)andonthealgorithm

used. A Monte Carlosimulation study is performed to determinethe average

performanceofthealgorithmforcomparisonwiththeCramer-Raobound. The

RootMeanSquare(RMS)MonteCarloerrorforeachxedtimeinstantislower

bounded bytheCramer-Raobound,

v u u t 1 M M X i=1 kx t ^ x i t k 2 & p trP t wherex^ i t

istheonestepaheadpredictionofthestatesattimetinMonteCarlo

runi.

Figure3showsa300sampleslongtrackoverapartoftheterrainmapfrom

Figure 2. Theaircraft travelsfrom rightto left. The Cramer-Raobound and

theobtainedRMS error from 1000Monte Carlosimulations withthe PMFis

showninFigure 4. ThePMFisinitiatedwithapriorwithlargesupportusing

alowresolutionofthegrid,thisyieldsfarfromoptimalperformanceduringthe

rsthalf ofthesimulations. However,asthegridresolutionincreasestheRMS

errordecreasesand whenthePMFresolution reachesasteady statelevel, the

lterperformancereachesthe optimalbound. Note also that both thebound

0

1000

2000

3000

4000

5000

6000

0

1000

2000

3000

4000

5000

6000

0

50

100

150

Figure 3: Thesimulationareaandthetruetrack,axesarelabeledin meters.

0

50

100

150

200

250

300

0

50

100

150

200

250

300

TheCramer-Raolowerbound MonteCarloRMSerror

Figure4: MonteCarlorootmeansquareerrorcompared withtheCramer-Rao

bound.

Monte Carloevaluation aboveshows that thegrid renement method used in

4 Simulation Evaluation

Theterrainmap usedin these simulationsisthesameterrain databaseovera

partofSwedenasisshowninFigure2. Acontourplotoverthisterrainmapand

thetruesimulatedaircraft ightpathisshowninFigure5. Theaircraftstarts

10

20

30

40

50

60

70

80

90

100

10

20

30

40

50

60

70

80

90

100

Figure5: Thesimulationtrackovertheterraindatabase.

headingsouth,after afew turnsovertheroughpartofthe mapit iesovera

partoftheBalticseaandthenturnsbackandcompletesthecounter-clockwise

lap. Thesimulatedaircrafttrack,theINSmeasurementsandtheradaraltimeter

measurementshaveallbeengenerated in anadvanced realisticsimulatorused

by the navigation systems development department at Saab Dynamics. The

track has a duration of 25 minutes and is sampled at a rate of 10 Hz. The

aircrafthasan averagespeedofMach 0:55,andthemanoeuvresaresimulated

ascoordinatedturns.

The INSposition estimate x^ 0

distributioncenteredat theerroneousINSestimate p(x 0 )=N(^x 0 ;1000 2 I 2 ): (11)

TheinitialgridresolutionusedinthePMFtosamplethisfunctionisÆ=200m.

The dead-reckoningdrift in theINS is simulatedas aconstant bias of 1m/s

in each channel. Thedistribution used in the algorithm to model this driftis

Gaussian p vt

(
) =N(0;4I 2

). The choice of Gaussian distributions has proven

successful in the simulations but any other suitable distribution that better

modelstheposition driftandtheinitial uncertaintymaybeused. Thereisno

restrictioninthePMFtoGaussiannoises: theonlyassumptionisthatthenoise

canberegardedaswhite.

Dierent sensor models are used when generating the simulated

measure-ments. Dependingontheterraincategorybeneaththeaircraftatthemeasuring

instant,boththebiasandthevarianceoftheradaraltimeterareadjusted. For

example, yingoverdenseforesttheradaraltimeterhasabiasof19m witha

largevariance. Additionalnoiseisadded tothemeasurementtosimulate that

theradaraltimetermeasurementperformancedegradeswithincreasingground

clearancedistance. Thedensityusedto capturetheseeects inthePMF

algo-rithmisamixtureoftwoGaussiandistributions,

p et

(
)=0:8N(0;2)+0:2N(15;9):

Thischoicecanbeinterpretedasontheaverageeveryfthmeasurementbeing

biased due to re ection in trees or buildings. Thetruncation and resampling

parametersusedin thePMFare,

"=10 3 ; N 0 =1000; N 1 =5000:

Thesimulationresultfromtherstthreerecursionsisdepictedin Figure6.

StartingwiththeGaussianprior(11),therstmeasurementampliesthe

prob-abilityinseveralregionsandremovessamplesoflowprobability. Afterthe

sec-ondrecursion,thegridresolutionisincreasedto100mandthethirdrecursion

removes even more samples and a single peak of the density shows the most

probableaircraftpositionwhiletheuncertaintystillisratherlarge. The

Prior

Recursion2 Recursion3

Figure6: Therstthreerecursionsofthealgorithm

support of each of thelter densities. Theirregular shapesof these densities

showshowtheunstructurednonlinearterraingivesalterdensitywhichishard

to approximate with smooth functions orlocal linearizations. Figure 7shows

the estimation error along the simulation track. Here it is obvious that the

performancedependsonthecoveredterrain. Theerrorconvergesrapidlyfrom

the initial error of morethan 1 km down to an error less than 30 m. When

theaircraftreachestheBalticseathemeasurementshavelittleinformationand

theerrorincreaseswiththedriftoftheINS.Oncebackoverland,theestimate

accuracyincreasesand atrendtowardsworseperformanceis visible when the

aircraftcoversthelowinformativeareasofthemapduringthenalpartofthe

lap. The resolution of the grid is automatically adjusted and varies between

200m and 0.78 m along the simulation track. A common navigation

perfor-manceparameteris thecircular error probable(CEP)which is themedian of

0

5000

10000

15000

10

−1

10

0

10

1

10

2

10

3

Figure7: Estimationerroralongthesimulationtrack,inlogarithmicscale.

comparison,in[12]anerrorof50mCEPisreportedandin[6]avalueof75m

isobtained. Itshouldberemarkedthatboththesevaluesarefoundduringeld

testsandnotsimulations.

5 Conclusion

Theperformanceofterrainnavigationdependsonthesizeoftheterraingradient

inthearea. Thepoint-masslterdescribedinthisworkyieldsanapproximate

Bayesiansolutionthat iswellsuited fortheunstructurednonlinearestimation

problem in terrain navigation. It recursivelypropagates adensity function of

theaircraftposition. Theshapeofthepoint-massdensityre ectstheestimate

quality, this information is crucial in navigation applications where estimates

from dierent sources often are fused in a central lter. The Monte Carlo

simulations show that the approximation can reach the optimal performance

andtherealisticsimulationsinSection4showthatthenavigationperformance

isveryhighcomparedwithotheralgorithmsandthatthepoint-massltersolves

therecursiveestimationproblemforallthetypesofterraincoveredinthetest.

ThemainadvantagesofthePMFisthatitworksformanykindsof

parameters. Themaindisadvantageisthatitcannotsolveestimationproblems

of very high dimension since the computational complexity of the algorithm

increasesdrastically with the dimension of the statespace. The

implementa-tionused inthisworkshowsreal-timeperformancefortwodimensional andin

somecasesthree dimensional models, but higher statedimensionsare usually

intractable.

6 Acknowledgment

TheworkhasbeenpartlysponsoredbyISIS,aNUTEKCompetenceCenterat

LinkopingUniversity. Thesimulationdataandtheterraindatabasehavebeen

providedbySaabDynamics,LinkopingSweden. Valuabledetailedexplanations

about the navigation application have been provided by several employees of

Saab,Linkoping.

References

[1] B.D.O. Anderson and J.B.Moore. Optimal Filtering. Prentice Hall,

En-glewoodClis,NJ,1979.

[2] W. Baker and R. Clem. Terrain contour matching [TERCOM] primer.

TechnicalReportASP-TR-77-61,AeronauticalSystems Division,

Wright-PattersonAFB,Aug.1977.

[3] N.Bergman. BayesianInferenceinTerrainNavigation. LinkopingStudies

in ScienceandTechnology.ThesisNo649,1997.

[4] N.Bergman.OntheCramer-Raoboundforterrain-aidednavigation.T

ech-nicalReport LiTH-ISY-R-1970,Dept.ofEE,LinkopingsUniversity,1997.

[5] N. Bergman, L.Ljung, and F.Gustafsson. Point-masslterand

Cramer-Rao bound for terrain-aided navigation. In Proc. 36:th IEEE Conf. on

AFTI/F-16Aircraft.JournalofTheInstituteofNavigation,35(2):161{175,

1988.

[7] R.S.BucyandK.D.Senne. Digitalsynthesisofnonlinearlters.

Automat-ica,7:287{298,1971.

[8] R. Enns and D. Morrell. Terrain-aided navigation using the Viterbi

al-gorithm. Journal of Guidance, Control and Dynamics, 18(6):1444{1449,

1995.

[9] J.P.Golden. Terraincontourmatching(TERCOM):acruisemissile

guid-ance aid. InT.F.Wiener,editor,Image Processingfor Missile Guidance,

volume238,pages10{18.SPIE,1980.

[10] A.J. Henley. Terrainaidednaviagtion{currentstatus,techniquesfor at

terrain andreferencedata requirements. InProc. ofIEEE Pos., Loc.and

Nav. Symp.(PLANS),LasVegas,1990.

[11] C. Hewitt. Theuseof terraindatabasesforavionicsystems. InIEE

Col-loquium on Terrain databases and their use in navigation and collision

avoidance, 1995.

[12] J.A.Hollowell. Heli/SITAN: Aterrainreferencednavigationalgorithmfor

helicopters. LasVegas,1990.IEEEPos.,Loc.andNav.Symp.(PLANS).

[13] L.D.Hostetler. Optimalterrain-aidednavigationsystems. InAIAA

Guid-ance andControl Conference,PaloAlto,CA, 1978.

[14] M. Kaytonand W.Fried,editors. Avionics Navigation Systems. 2nd

edi-tion,1997.

[15] S.C. Kramerand H.W. Sorenson. Bayesianparameter estimation. IEEE

Transactions onAutomatic Control,33:217{222,1988.

[16] G.M.Siouris. AerospaceAvionicsSystems. AcademicPress,1993.

[17] H.W. Sorenson. Recursive estimation for nonlineardynamic systems. In

J.C.Spall,editor,BayesianAnalysisofTimeSeriesandDynamicModels,