Transcription

1 ANewExtensionoftheKalmanFiltertoNonlinear SimonJ.JulierSystems TheRoboticsResearchGroup,DepartmentofEngineeringScience,TheUniversityofOxford Oxford,OX13PJ,UK,Phone: ,Fax: JereyK.Uhlmann optimality,tractabilityandrobustness.however,theapplicationofthekftononlinearsystemscanbedicult. TheKalmanlter(KF)isoneofthemostwidelyusedmethodsfortrackingandestimationduetoitssimplicity, ABSTRACT widelyusedlteringstrategy,overthirtyyearsofexperiencewithithasledtoageneralconsensuswithinthe trackingandcontrolcommunitythatitisdiculttoimplement,diculttotune,andonlyreliableforsystems whicharealmostlinearonthetimescaleoftheupdateintervals. ThemostcommonapproachistousetheExtendedKalmanFilter(EKF)whichsimplylinearisesallnonlinear modelssothatthetraditionallinearkalmanltercanbeapplied.althoughtheekf(initsmanyforms)isa sampledpointscanbeusedtoparameterisemeanandcovariance,theestimatoryieldsperformanceequivalentto thekfforlinearsystemsyetgeneraliseselegantlytononlinearsystemswithoutthelinearisationstepsrequired bytheekf.weshowanalyticallythattheexpectedperformanceofthenewapproachissuperiortothatofthe EKFand,infact,isdirectlycomparabletothatofthesecondorderGausslter.Themethodisnotrestricted Inthispaperanewlinearestimatorisdevelopedanddemonstrated.Usingtheprinciplethatasetofdiscretely toassumingthatthedistributionsofnoisesourcesaregaussian.wearguethattheeaseofimplementationand moreaccurateestimationfeaturesofthenewlterrecommenditsuseovertheekfinvirtuallyallapplications. Keywords:Navigation,estimation,non-linearsystems,Kalmanltering,sampling. mustbeestimatedfromnoisysensorinformation,somekindofstateestimatorisemployedtofusethedatafrom Filteringandestimationaretwoofthemostpervasivetoolsofengineering.Wheneverthestateofasystem 1 INTRODUCTION andobservationmodelsarelinear,theminimummeansquarederror(mmse)estimatemaybecomputedusing dierentsensorstogethertoproduceanaccurateestimateofthetruesystemstate.whenthesystemdynamics thekalmanlter.however,inmostapplicationsofinterestthesystemdynamicsandobservationequationsare nonlinearandsuitableextensionstothekalmanlterhavebeensought.itiswell-knownthattheoptimalsolution tothenonlinearlteringproblemrequiresthatacompletedescriptionoftheconditionalprobabilitydensityis anumberofsuboptimalapproximationshavebeenproposed6?8;13;16;21. maintained14.unfortunatelythisexactdescriptionrequiresapotentiallyunboundednumberofparametersand

2 TheEKFappliestheKalmanltertononlinearsystemsbysimplylinearisingallthenonlinearmodelssothat thetraditionallinearkalmanlterequationscanbeapplied.however,inpractice,theuseoftheekfhastwo well-knowndrawbacks: ProbablythemostwidelyusedestimatorfornonlinearsystemsistheextendedKalmanlter(EKF)20;22. 1.Linearisationcanproducehighlyunstableltersiftheassumptionsoflocallinearityisviolated. linearsystems,yetgeneraliseselegantlytononlinearsystemswithoutthelinearisationstepsrequiredbythe 2.ThederivationoftheJacobianmatricesarenontrivialinmostapplicationsandoftenleadtosignicant EKF.Thefundamentalcomponentofthislteristheunscentedtransformationwhichusesasetofappropriately InthispaperwederiveanewlinearestimatorwhichyieldsperformanceequivalenttotheKalmanlterfor implementationdiculties. chosenweightedpointstoparameterisethemeansandcovariancesofprobabilitydistributions.wearguethat theexpectedperformanceofthenewapproachissuperiortothatoftheekfand,infact,isdirectlycomparable tothatofthesecondordergausslter.further,thenatureofthetransformissuchthattheprocessand algorithmhassuperiorimplementationpropertiestotheekf.wedemonstratethedierencesinperformancein observationmodelscanbetreatedas\blackboxes".itisnotnecessarytocalculatejacobiansandsothe Kalmanltertononlinearsystems.Wearguethattheprincipleproblemistheabilitytopredictthestateof thenewlterrecommenditsuseovertheekfinvirtuallyallapplications. anexampleapplication,andwearguethattheeaseofimplementationandmoreaccurateestimationfeaturesof thesystem.section3introducestheunscentedtransformation.itspropertiesareanalysedandafullltering algorithm,whichincludestheeectsofprocessnoise,isdeveloped.insection4anexampleispresented.using realisticdata,thecomparisonoftheunscentedlterandekfforthetrackingofareentrybodyisconsidered. Thestructureofthispaperisasfollows.InSection2wedescribetheproblemstatementforapplyinga ConclusionsaredrawninSection5.Acompanionpaper10,extendsthebasicmethodandshowsthatjudiciously selectingadditionalpointscanleadtoanydesiredlevelofaccuracyforanygivenpriordistribution. 2.1ProblemStatement 2 ESTIMATIONINNONLINEARSYSTEMS WewishtoapplyaKalmanltertoanonlineardiscretetimesystemoftheform wherex(k)isthen-dimensionalstateofthesystemattimestepk,u(k)istheinputvector,v(k)istheqdimensionalstatenoiseprocessvectorduetodisturbancesandmodellingerrors,z(k)istheobservationvector (2) x(k+1)=f[x(k);u(k);v(k);k]; z(k)=h[x(k);u(k);k]+w(k); (1) andw(k)isthemeasurementnoise.itisassumedthatthenoisevectorsv(k)andw(k),arezero-meanand \predictor-corrector"structure.let^x(ijj)betheestimateofx(i)usingtheobservationinformationinformation TheKalmanlterpropagatesthersttwomomentsofthedistributionofx(k)recursivelyandhasadistinctive Ev(i)vT(j)=ijQ(i);Ew(i)wT(j)=ijR(i);Ev(i)wT(j)=0;8i;j: uptoandincludingtimej,zj=[z(1);:::;z(j)].thecovarianceofthisestimateisp(ijj).givenanestimate predictedquantitiesaregivenbytheexpectations ^x(kjk),thelterrstpredictswhatthefuturestateofthesystemwillbeusingtheprocessmodel.ideally,the P(k+1jk)=Ehfx(k+1)?^x(k+1jk)gfx(k+1)?^x(k+1jk)gTjZki: ^x(k+1jk)=ef[x(k);u(k);v(k);k]jzk (4) (3)

3 ofx(k),conditiononzk,isknown.however,thisdistributionhasnogeneralformandapotentiallyunbounded niteandtractablenumberofparametersneedbepropagated.itisconventionallyassumedthatthedistribution ofx(k)isgaussianfortworeasons.first,thedistributioniscompletelyparameterisedbyjustthemeanand numberofparametersarerequired.inmanyapplications,thedistributionofx(k)isapproximatedsothatonlya Whenf[]andh[]arenonlinear,theprecisevaluesofthesestatisticscanonlybecalculatedifthedistribution informative3. covariance.second,giventhatonlythersttwomomentsareknown,thegaussiandistributionistheleast thekalmanlteralinearupdateruleisspeciedandtheweightsarechosentominimisethemeansquarederror oftheestimate.theupdateruleis Theestimate^x(k+1jk+1)isgivenbyupdatingthepredictionwiththecurrentsensormeasurement.In P(k+1jk+1)=P(k+1jk)?W(k+1)P(k+1jk)WT(k+1) ^x(k+1jk+1)=^x(k+1jk)+w(k+1)(k+1); Itisimportanttonotethattheseequationsareonlyafunctionofthepredictedvaluesofthersttwomoments ofx(k)andz(k).therefore,theproblemofapplyingthekalmanltertoanonlinearsystemistheabilityto W(k+1)=Px(k+1jk)P?1 (k+1)=z(k+1)?^z(k+1jk) predictthersttwomomentsofx(k)andz(k).thisproblemisaspeciccaseofageneralproblem tobe (k+1jk): abletocalculatethestatisticsofarandomvariablewhichhasundergoneanonlineartransformation. form.supposethatxisarandomvariablewithmeanxandcovariancepxx.asecondrandomvariable,yis 2.2TheTransformationofUncertainty relatedtoxthroughthenonlinearfunction Theproblemofpredictingthefuturestateorobservationofthesystemcanbeexpressedinthefollowing WewishtocalculatethemeanyandcovariancePyyofy. Thestatisticsofyarecalculatedby(i)determiningthedensityfunctionofthetransformeddistributionand y=f[x]: (5) closedformsolutionsexist.however,suchsolutionsdonotexistingeneralandapproximatemethodsmustbe (ii)evaluatingthestatisticsfromthatdistribution.insomespecialcases(forexamplewhenf[]islinear)exact, ecientandunbiased. used.inthispaperweadvocatethatthemethodshouldyieldconsistentstatistics.ideally,theseshouldbe holds.thisconditionisextremelyimportantforthevalidityofthetransformationmethod.ifthestatisticsare Thetransformedstatisticsareconsistentiftheinequality notconsistent,thevalueofpyyisunder-estimated.ifakalmanlterusestheinconsistentsetofstatistics,it willplacetoomuchweightontheinformationandunderestimatethecovariance,raisingthepossibilitythat Pyy?Ehfy?ygfy?ygTi0 (6) ofthelefthandsideofequation6shouldbeminimised.finally,itisdesirablethattheestimateisunbiasedor thelterwilldiverge.byensuringthatthetransformationisconsistent,thelterisguaranteedtobeconsistent greatlyinexcessoftheactualmeansquarederror.itisdesirablethatthetransformationisecient thevalue aswell.however,consistencydoesnotnecessaryimplyusefulnessbecausethecalculatedvalueofpyymightbe consideringthetaylorseriesexpansionofequation5aboutx.thisseriescanbeexpressed(usingratherinformal ye[y]. Theproblemofdevelopingaconsistent,ecientandunbiasedtransformationprocedurecanbeexaminedby

4 notation)as: wherexisazeromeangaussianvariablewithcovariancepxx,andrnfxnistheappropriatenthorderterm f[x]=f[x+x] inthemultidimensionaltaylorseries.takingexpectations,itcanbeshownthatthetransformedmeanand =f[x]+rfx+12r2fx2+13!r3fx3+14!r4fx4+ (7) covariancearey=f[x]+12r2fpxx+12r4fex4+ Pyy=rfPxx(rf)T+1 13!r3fEx4(rf)T+: 24!r2fEx4?Ex2Pyy?EPyyx2+P2yy(r2f)T+ (8) Inotherwords,thenthordertermintheseriesforxisafunctionofthenthordermomentsofxmultipliedby thenthorderderivativesoff[]evaluatedatx=x.ifthemomentsandderivativescanbeevaluatedcorrectly (9) uptothenthorder,themeaniscorrectuptothenthorderaswell.similarcommentsholdforthecovariance byaprogressivelysmallerandsmallerterm,thelowestordertermsintheseriesarelikelytohavethegreatest equationaswell,althoughthestructureofeachtermismorecomplicated.sinceeachtermintheseriesisscaled impact.therefore,thepredictionprocedureshouldbeconcentratedonevaluatingthelowerorderterms. thisassumption, LinearisationassumesthatthesecondandhigherordertermsofxinEquation7canbeneglected.Under Pyy=rfPxx(rf)T: y=f[x]; (10) thesecondandhigherordertermsinthemeanandfourthandhigherordertermsinthecovariancearenegligible. ComparingtheseexpressionswithEquations8and9,itisclearthattheseapproximationsareaccurateonlyif However,inmanypracticalsituationslinearisationintroducessignicantbiasesorerrors.Anextremelycommon (11) andimportantproblemisthetransformationofinformationbetweenpolarandcartesiancoordinatesystems10;15. Thisisdemonstratedbythesimpleexamplegiveninthenextsubsection. 2.3Example returnspolarinformation(rangerandbearing)andthisistobeconvertedtoestimatetocartesiancoordinates. Thetransformationis: Supposeamobilerobotdetectsbeaconsinitsenvironmentusingarange-optimisedsonarsensor.Thesensor Thereallocationofthetargetis(0;1).Thedicultywiththistransformationarisesfromthephysicalproperties xy=rcos ofthesonar.fairlygoodrangeaccuracy(with2cmstandarddeviation)istradedotogiveaverypoorbearing rsinwithrf=cos?rsin sinrcos: tobeviolated. measurement(standarddeviationof15).thelargebearinguncertaintycausestheassumptionoflocallinearity

5 comparedwiththosecalculatedbythetruestatistics linearisation,itsvaluesofthestatisticsof(x;y)were Toappreciatetheerrorswhichcanbecausedby isticswereobtained.theresultsareshowninfigure1. usedtoensurethataccurateestimatesofthetruestat- totheslowconvergenceofrandomsamplingmethods, anextremelylargenumberofsamples(3:5106)were whicharecalculatedbymontecarlosimulation.due 1.04 Thisgureshowsthemeanand1contoursforwhich arecalculatedbyeachmethod.the1contouristhe locusofpointsfy:(y?y)p? graphicalrepresentationofthesizeandorientationof y(y?y)=1gandisa isextremelysubstantial.linearisationerrorseectivelyintroduceanerrorwhichisover1.5timesthelatesthemeanatandtheuncertaintyellipseis 0.9 thepositionis1mwhereasinrealityitis96.7cm.this therangedirection,wherelinearisationestimatesthat Pyy.Ascanbeseen,thelinearisedtransformationis biasedandinconsistent.thisismostpronouncedinfigure1:themeanandstandarddeviationel- lipsesfortheactualandlinearisedformofthe 0.94 uncertaintyellipseissolid.linearisationcalcu- transformation.thetruemeanisatandthe 0.92 mittedeachtimeacoordinatetransformationtakes dashed place.eveniftherewerenobias,thetransformation itself,thesameerrorwiththesamesignwillbecom- isabiaswhicharisesfromthetransformationprocess standarddeviationoftherangemeasurement.sinceit True mean: x EKF mean: o isinconsistent.itsellipseisnotlongenoughintherdirection.infact,thenatureoftheinconsistencycompounds ismuchsmallerthanthetruevalue. theproblemofthebiased-ness:notonlyistheestimateorrinerror,butalsoitsestimatedmeansquarederror sizeofthetransformedcovariance.thisisonepossibleofwhyekfsaresodiculttotune sucientnoise mustbeintroducedtoosetthedefectsoflinearisation.however,introducingstabilisingnoiseisanundesirable solutionsincetheestimateremainsbiasedandthereisnogeneralguaranteethatthetransformedestimateremains consistentorecient.amoreaccuratepredictionalgorithmisrequired. Inpracticetheinconsistencycanberesolvedbyintroducingadditionalstabilisingnoisewhichincreasesthe 3.1TheBasicIdea 3 THEUNSCENTEDTRANSFORM foundedontheintuitionthatitiseasiertoapproximateagaussiandistributionthanitistoapproximatablewhichundergoesanonlineartransformation.itis Theunscentedtransformationisanew,novel methodforcalculatingthestatisticsofarandomvari- anarbitrarynonlinearfunctionortransformation23. (orsigmapoints)arechosensothattheirsamplemean TheapproachisillustratedinFigure2.Asetofpoints functionisappliedtoeachpointinturntoyieldacloud andsamplecovariancearexandpxx.thenonlinear oftransformedpointsandyandpyyarethestatisticsofthetransformedpoints.althoughthismethod baresasupercialresemblancetomontecarlo-typeform. Figure2:Theprincipleoftheunscentedtrans- Nonlinear Transformation

6 methods,thereisanextremelyimportantandfundamentaldierence.thesamplesarenotdrawnatrandom butratheraccordingtoaspecic,deterministicalgorithm.sincetheproblemsofstatisticalconvergencearenot anissue,highorderinformationaboutthedistributioncanbecapturedusingonlyaverysmallnumberofpoints. pointsgivenby Then-dimensionalrandomvariablexwithmeanxandcovariancePxxisapproximatedby2n+1weighted Xi+n=x?p(n+)PxxiWi+n=1=2(n+) X0 =x+p(n+)pxxiwi W0 =1=2(n+) ==(n+) where2<,p(n+)pxxiistheithroworcolumnofthematrixsquarerootof(n+)pxxandwiisthe (12) weightwhichisassociatedwiththeithpoint.thetransformationprocedureisasfollows: 1.Instantiateeachpointthroughthefunctiontoyieldthesetoftransformedsigmapoints, 2.Themeanisgivenbytheweightedaverageofthetransformedpoints, Yi=f[Xi]: 3.Thecovarianceistheweightedouterproductofthetransformedpoints, y=2nxi=0wiyi: (13) Thepropertiesofthisalgorithmhavebeenstudiedindetailelsewhere9;12andwepresentasummaryofthe Pyy=2nXi=0WifYi?ygfYi?ygT: (14) resultshere: 1.Sincethemeanandcovarianceofxarecapturedpreciselyuptothesecondorder,thecalculatedvalues ofthemeanandcovarianceofyarecorrecttothesecondorderaswell.thismeansthatthemeanis orderofaccuracy.however,therearefurtherperformancebenets.sincethedistributionofxisbeing calculatedtoahigherorderofaccuracythantheekf,whereasthecovarianceiscalculatedtothesame 2.Thesigmapointscapturethesamemeanandcovarianceirrespectiveofthechoiceofmatrixsquareroot approximatedratherthanf[],itsseriesexpansionisnottruncatedataparticularorder.itcanbeshown thattheunscentedalgorithmisabletopartiallyincorporateinformationfromthehigherorders,leadingto evengreateraccuracy. 3.Themeanandcovariancearecalculatedusingstandardvectorandmatrixoperations.Thismeansthatthe whichisused.numericallyecientandstablemethodssuchasthecholeskydecomposition18canbeused. 4.providesanextradegreeoffreedomto\netune"thehigherordermomentsoftheapproximation,and notnecessarytoevaluatethejacobianswhichareneededinanekf. algorithmissuitableforanychoiceofprocessmodel,andimplementationisextremelyrapidbecauseitis canbeusedtoreducetheoverallpredictionerrors.whenx(k)isassumedgaussian,ausefulheuristicisto selectn+=3.ifadierentdistributionisassumedforx(k)thenadierentchoiceofmightbemore appropriate.

7 5.Althoughcanbepositiveornegative,anegativechoiceofcanleadtoanon-positivesemideniteestimate densitydistributions8;16;21.inthissituation,itispossibletouseamodiedformofthepredictionalgorithm. Themeanisstillcalculatedasbefore,butthe\covariance"isevaluatedaboutX0(k+1jk):Itcanbe ofpyy.thisproblemisnotuncommonformethodswhichapproximatehigherordermomentsorprobability shownthatthemodiedformensurespositivesemi-denitenessand,inthelimitas(n+)!0, Inotherwords,thealgorithmcanbemadetoperformexactlylikethesecondOrderGaussFilter,but withouttheneedtocalculatejacobiansorhessians. (n+)!0y=f[x]+12r2fpxx;lim (n+)!0pyy=rfpxx(rf)t: meansand1contoursdeterminedbythedierent transformcanbeseeninfigure3whichshowsthe methods.thetruemeanliesatwithadottedcovariancecontour.thepositionoftheunscentedmean Theperformancebenetsofusingtheunscented value onthescaleofthegraph,thetwopointslieon topofoneanother.further,theunscentedtransform isindicatedbya?anditscontourissolid.thelinearisedmeanisatandusedadashedcontour.ascanbe seentheunscentedmeanvalueisthesameasthetrue thanthetruecontourintherdirection. isconsistent infact,itscontourisslightlylarger isbettersuitedthanlinearisationforlteringapplica- andeaseofimplementation,theunscentedtransform tions.indeed,sinceitcanpredictthemeanandcovari- ancewithsecondorderaccuracy,anylterwhichuses Givenitspropertiesofsuperiorestimationaccuracy Figure3:Theunscentedtransformasappliedto theunscentedtransformtothelteringproblemanddevelopstheunscentedlter. ans.thenextsubsectionexaminestheapplicationof doesnotrequirethederivationofjacobiansorhessi- theunscentedtransformwillhavethesameperformanceasthetruncatedsecondordergaussfilter1butthemeasurementexample. 3.2TheUnscentedFilter ThetransformationprocesseswhichoccurinaKalmanlterconsistofthefollowingsteps: Predictthenewstateofthesystem^x(k+1jk)anditsassociatedcovarianceP(k+1jk).Thisprediction Predicttheexpectedobservation^z(k+1jk)andtheinnovationcovarianceP(k+1jk).Thisprediction musttakeaccountoftheeectsofprocessnoise. Finally,predictthecross-correlationmatrixPxz(k+1jk): shouldincludetheeectsofobservationnoise. models.first,thestatevectorisaugmentedwiththeprocessandnoisetermstogiveanna=n+qdimensional Thesestepscanbeeasilyaccommodatedbyslightlyrestructuringthestatevectorandprocessandobservation True mean: x EKF mean: o Kappa mean: +

8 1.ThesetofsigmapointsarecreatedbyapplyingEquation12totheaugmentedsystemgivenbyEquation15. 2.Thetransformedsetisgivenbyinstantiatingeachpointthroughtheprocessmodel, 3.Thepredictedmeaniscomputedas Xi(k+1jk)=f[Xai(kjk);u(k);k]: 4.Andthepredictedcovarianceiscomputedas ^x(k+1jk)=2na Xi=0WiXai(k+1jk): 5.Instantiateeachofthepredictionpointsthroughtheobservationmodel, P(k+1jk)2na Xi=0WifXi(k+1jk)?^x(k+1jk)gfXi(k+1jk)?^x(k+1jk)gT 6.Thepredictedobservationiscalculatedby Zi(k+1jk)=h[Xi(k+1jk);u(k);k] 7.Sincetheobservationnoiseisadditiveandindependent,theinnovationcovarianceis ^z(k+1jk)=2na Xi=1WiZi(k+1jk): 8.Finallythecrosscorrelationmatrixisdeterminedby P(k+1jk)=R(k+1)+2na Xi=0WifZi(kjk?1)?^z(k+1jk)gfZi(kjk?1)?^z(k+1jk)gT Pxz(k+1jk)=2na Xi=0WifXi(kjk?1)?^x(k+1jk)gfZi(kjk?1)?^z(k+1jk)gT vector, Box3.1:Thepredictionalgorithmusingtheunscentedtransform. Theprocessmodelisrewrittenasafunctionofxa(k), xa(k)=x(k) x(k+1)=f[xa(k);u(k);k] v(k): andtheunscentedtransformuses2na+1sigmapointswhicharedrawnfrom Thematricesontheleadingdiagonalarethecovariancesando-diagonalsub-blocksarethecorrelations ^xa(kjk)=^x(kjk) 0q1andPa(kjk)=P(kjk)Pxv(kjk) Pxv(kjk) Q(k): (15) betweenthestateerrorsandtheprocessnoises.althoughthismethodrequirestheuseofadditionalsigma points,itmeansthattheeectsoftheprocessnoise(intermsofitsimpactonthemeanandcovariance)are introducedwiththesameorderofaccuracyastheuncertaintyinthestate.theformulationalsomeansthat

9 expressionfortheunscentedtransformisgivenbytheequationsinbox3.1. correlatednoisesources(whichcanariseinschmidt-kalmanlters19)canbeimplementedextremelyeasily.the withprocessand/orobservationnoise,thentheaugmentedvectorisexpandedtoincludetheobservationterms. agivenapplication.forexample,iftheobservationnoiseisintroducedinanonlinearfashion,oriscorrelated Thissectionhasdevelopedtheunscentedtransformsothatitcanbeusedinlteringandtrackingapplications. Variousextensionsandmodicationscanbemadetothisbasicmethodtotakeaccountofspecicdetailsof ThenextsectiondemonstratesitsbenetsovertheEKFforasampleapplication. 4 EXAMPLEAPPLICATION ofthebodyistobetrackedbyaradarwhichaccurately athighaltitudeandataveryhighspeed.theposition lustratedinfigure4:avehicleenterstheatmosphere measuresrangeandbearing.thistypeofproblemhas Inthissectionweconsidertheproblemwhichisil- whichact.themostdominantisaerodynamicdrag, particularlystressfulforltersandtrackersbecauseof beenidentiedbyanumberofauthors1;2;5;17asbeing actonthevehicle.therearethreetypesofforces thestrongnonlinearitiesexhibitedbytheforceswhich tialnonlinearvariationinaltitude.thesecondtypeof whichisafunctionofvehiclespeedandhasasubstan- forceisgravitywhichacceleratesthevehicletowards atmosphereincreases,drageectsbecomeimportant thecentreoftheearth.thenalforcesarerandom buetingterms.theeectoftheseforcesgivesatrajectoryoftheformshowninfigure4:initiallythe almostvertical.thetrackingproblemismademore dicultbythefactthatthedragpropertiesofthe andthevehiclerapidlydeceleratesuntilitsmotionis trajectoryisalmostballisticbutasthedensityofthe vehiclemightbeonlyverycrudelyknown. isthesamplevehicletrajectoryandthesolidline Figure4:Thereentryproblem.Thedashedline trackanobjectwhichexperiencesasetofcomplicated,highlynonlinearforces.thesedependonthecurrent Insummary,thetrackingsystemshouldbeabletooftheradarismarkedbya. isaportionoftheearth'ssurface.theposition positionandvelocityofthevehicleaswellasoncertaincharacteristicswhicharenotknownprecisely.thelter's aerodynamicproperties(x5).thevehiclestatedynamicsare statespaceconsistsofthepositionofthebody(x1andx2),itsvelocity(x3andx4)andaparameterofits _x1(k)=x3(k) _x2(k)=x4(k) _x3(k)=d(k)x3(k)+g(k)x1(k)+v1(k) _x4(k)=d(k)x4(k)+g(k)x2(k)+v2(k) _x5(k)=v3(k) (16) whered(k)isthedrag-relatedforceterm,g(k)isthegravity-relatedforcetermandv(k)aretheprocessnoise terms.deningr(k)=px21(k)+x2(k)asthedistancefromthecentreoftheearthandv(k)=px23(k)+x24(k) x 2 (km) x 1 (km)

10 asabsolutevehiclespeedthenthedragandgravitationaltermsare D(k)=?(k)exp[R0?R(k)] V(k);G(k)=?Gm0 Forthisexampletheparametervaluesare0=?0:59783,H0=13:406,Gm0=3: andR0=6374and and(k)=0expx5(k): r3(k) reecttypicalenvironmentalandvehiclecharacteristics2.theparameterisationoftheballisticcoecient,(k), reectstheuncertaintyinvehiclecharacteristics5.0istheballisticcoecientofa\typicalvehicle"anditis scaledbyexpx5(k)toensurethatitsvalueisalwayspositive.thisisvitalforlterstability. andbearingatafrequencyof10hz,where Themotionofthevehicleismeasuredbyaradarwhichislocatedat(xr;yr).Itisabletomeasureranger rr(k)=p(x1(k)?xr)2+(x2(k)?yr)2+w1(k) w1(k)andw2(k)arezeromeanuncorrelatednoiseprocesseswithvariancesof1mand17mradrespectively4.the highupdaterateandextremeaccuracyofthesensormeansthatalargequantityofextremelyhighqualitydatais (k)=tan?1x2(k)?yr x1(k)?xr+w2(k) availableforthelter.thebearinguncertaintyissucientlythattheekfisabletopredictthesensorreadings accuratelywithverylittlebias. Thetrueinitialconditionsforthevehicleare x(0)= 0 B@?1:8093?6: :4 349:14 0: CAandP(0)= ?610?6 10?6 Inotherwords,thevehicle'scoecientistwicethenominalcoecient ? : Thevehicleisbuetedbyrandomaccelerations, Q(k)=242:406410?5 0 2:406410?50 0 Theinitialconditionsassumedbythelterare, 3 5 ^x(0j0)= 0 B@?1:8093?6: :4 349:141CAandP(0j0)= ?610?6 10?6 Thelterusesthenominalinitialconditionand,toosetfortheuncertainty,thevarianceonthisinitialestimate ? : is1ḃothlterswereimplementedindiscretetimeandobservationsweretakenatafrequencyof10hz.however, duetotheintensenonlinearitiesofthevehicledynamicsequations,theeulerapproximationofequation16was onlyvalidforsmalltimesteps.theintegrationstepwassettobe50mswhichmeantthattwopredictionswere

11 10 0 Mean squared error and variance of x Mean squared error and variance of x Mean squared error and variance of x Position variance km (a)resultsforx1. (b)resultsforx Figure5:ThemeansquarederrorsandestimatedcovariancescalculatedbyanEKFandan unscentedlter.inallthegraphs,thesolidlineisthemeansquarederrorcalculatedbytheekf, (c)resultsforx Time s Time s errorandthedot-dashedlineitsestimatedcovariance. andthedottedlineisitsestimatedcovariance.thedashedlineistheunscentedmeansquared Time s madeperupdate.fortheunscentedlter,eachsigmapointwasappliedthroughthedynamicsequationstwice. error(thediagonalelementsofp(kjk))againstactualmeansquaredestimationerror(whichisevaluatedusing FortheEKF,itwasnecessarytoperformaninitialpredictionstepandre-linearisebeforethesecondstep. 100MonteCarlosimulations).Onlyx1,x3andx5areshown theresultsforx2aresimilartox1,andx4isthe sameasthatforx3.inallcasesitcanbeseenthattheunscentedlterestimatesitsmeansquarederrorvery accurately,anditispossibletobecondentwiththelterestimates.theekf,however,ishighlyinconsistent: TheperformanceofeachlterisshowninFigure5.Thisgureplotstheestimatedmeansquaredestimation thepeakmeansquarederrorinx1is0:4km2,whereasitsestimatedcovarianceisoveronehundredtimessmaller. error.finally,itcanbeseenthatx5ishighlybiased,andthisbiasonlyslowlydecreasesovertime.thispoor performanceisthedirectresultoflinearisationerrors. Similarly,thepeakmeansquaredvelocityerroris3:410?4km2s?2whichisover5timesthetruemeansquared theneedtoconsistentlypredictthenewstateandobservationofthesystem.wehaveintroducedanewltering InthispaperwehavearguedthattheprincipledicultyforapplyingtheKalmanltertononlinearsystemsis 5 CONCLUSIONS advantagesovertheekf.first,itisabletopredictthestateofthesystemmoreaccurately.second,itismuch algorithm,calledtheunscentedlter.byvirtueoftheunscentedtransformation,thisalgorithmhastwogreat lessdiculttoimplement.thebenetsofthealgorithmweredemonstratedinarealisticexample. Inacompanionpaper11,weextendthedevelopmentoftheunscentedtransformandyieldageneralframeworkfor itsderivationandapplication.itisshownthatthenumberofsigmapointscanbeextendedtoyieldalterwhich matchesmomentsuptothefourthorder.thishigherorderextensioneectivelyde-biasesalmostallcommon nonlinearcoordinatetransformations. Thispaperhasconsideredonespecicformoftheunscentedtransformforoneparticularsetofassumptions. Velocity variance (km/s) 2 Coefficient variance

12 [1]M.Athans,R.P.WishnerandA.Bertolini.Suboptimalstateestimationforcontinuous-timenonlinearsystemsfrom discretenoisymeasurements.ieeetransactionsonautomaticcontrol,tac-13(6):504{518,october1968. REFERENCES [2]J.W.AustinandC.T.Leondes.StatisticallyLinearizedEstimationofReentryTrajectories.IEEETransactionson [3]D.E.Catlin.Estimation,controlandthediscretekalmanlter.InAppliedMathematicalSciences71,page84. [4]C.B.Chang,R.H.WhitingandM.Athans.OntheStateandParameterEstimationforManeuveringReentry AerospaceandElectronicSystems,AES-17(1):54{61,January1981. [5]P.J.Costa.AdaptiveModelArchitectureandExtendedKalman-BucyFilters.IEEETransactionsonAerospaceand Springer-Verlag,1989. [6]F.E.Daum.Newexactnonlinearlters.InJ.C.Spall,editor,BayesianAnalysisofTimeSeriesandDynamic Vehicles.IEEETransactionsonAutomaticControl,AC-22:99{105,February1977. [7]N.J.Gordon,D.J.SalmondandA.F.M.Smith.NovelApproachtoNonlinear/non-GaussianBayesianState ElectronicSystems,AES-30(2):525{533,April1994. [8]A.H.Jazwinski.StochasticProcessesandFilteringTheory.AcademicPress,1970. Models,chapter8,pages199{226.MarcelDrekker,Inc.,1988. [9]S.J.JulierandJ.K.Uhlmann.AGeneralMethodforApproximatingNonlinearTransformationsofProbability Estimation.IEEProceedings-F,140(2):107{113,April1993. [10]S.J.JulierandJ.K.Uhlmann.AConsistent,DebiasedMethodforConvertingBetweenPolarandCartesian Distributions.WWWhttp:// CoordinateSystems.InTheProceedingsofAeroSense:The11thInternationalSymposiumonAerospace/Defense [12]S.J.Julier,J.K.UhlmannandH.F.Durrant-Whyte.ANewApproachfortheNonlinearTransformationofMeans [11]S.J.Julier,J.K.UhlmannandH.F.Durrant-Whyte.ANewApproachforFilteringNonlinearSystems.InThe ProceedingsoftheAmericanControlConference,Seattle,Washington.,pages1628{1632,1995. Sensing,SimulationandControls,Orlando,Florida.SPIE,1997.Acquisition,TrackingandPointingXI. [13]H.J.Kushner.Approximationstooptimalnonlinearlters.IEEETransactionsonAutomaticControl,AC-12(5):546{ andcovariancesinlinearfilters.ieeetransactionsonautomaticcontrol,1996. [15]D.LerroandY.K.Bar-Shalom.TrackingwithDebiasedConsistentConvertedMeasurementsvs.EKF.IEEE [14]H.J.Kushner.Dynamicalequationsforoptimumnon-linearltering.JournalofDierentialEquations,3:179{190, 556,October1967. [16]P.S.Maybeck.StochasticModels,Estimation,andControl,volume2.AcademicPress,1982. [17]R.K.Mehra.AComparisonofSeveralNonlinearFiltersforReentryVehicleTracking.IEEETransactionson TransactionsonAerospaceandElectronicsSystems,AES-29(3):1015{1022,July1993. [18]W.H.Press,S.A.Teukolsky,W.T.VetterlingandB.P.Flannery.NumericalRecipesinC:TheArtofScientic [19]S.F.Schmidt.Applicationsofstatespacemethodstonavigationproblems.InC.T.Leondes,editor,Advanced AutomaticControl,AC-16(4):307{319,August1971. [20]H.W.Sorenson,editor.Kalmanltering:theoryandapplication.IEEE,1985. ControlSystems,volume3,pages293{340.AcademicPress,1966. Computing.CambridgeUniversityPress,2edition,1992. [21]H.W.SorensonandA.R.Stubberud.Non-linearlteringbyapproximationoftheaposterioridensity.International [22]J.K.Uhlmann.Algorithmsformultipletargettracking.AmericanScientist,80(2):128{141,1992. [23]J.K.Uhlmann.Simultaneousmapbuildingandlocalizationforrealtimeapplications.Technicalreport,University JournalofControl,8(1):33{51,1968. ofoxford,1994.transferthesis.