Robust Extended Kalman Filtering in Hybrid Positioning Applications Tommi PER AL A and - PDF document

RobustExtendedKalmanFilteringinHybridPositioningApplicationsTommiPERAandRobertPICHTampereUniversityofTechnology,FinlandEmail:(tommi.perala,robert.piche)@tut.“„TheKalman“lteranditsextensionshasbeenwidelystudiedandappliedinpositioning,inpartbecauseitslowcomputationalcomplexityiswellsuitedtosmallmobiledevices.Whilethese“ltersareaccurateforproblemswithsmallnonlinearitiesandnearlygaussiannoisestatistics,theycanperformverybadlywhentheseconditionsdonotprevail. resultsofsimulationtestsarediscussedinsectionV.II.MThestateofthesystemattimeismodelledasastochasticvariable.Theevolutionofthestateintimeisgivenbyastochasticdifferenceequationandtheinitialstateisassumedtobenormallydistributedwithmeanandcovariancematrix.Themeasurementsarerelatedtothestatebydescribesthenoiseinthestatedynamicsandthenoiseinthemeasurements.Inthispaperthenoisesareassumedtobewhitezero-meanmutuallyindependentrandomprocesses,andalsoindependentwiththeinitialstate.Letbethemeasurementsbeforetime.ThemeasurementsusedinthisworkareGPSpseudorangemeasurementsandtheirderivatives,andbasestationrangeandaltitudemeasurements.Becausethereisunknownclockbiasinthesatellitemeasurements,differencemeasurementsareusedinstead.Onesatelliteischosenasreferenceandallthedifferencesareformedwithrespecttoit.Letdenotethepositionoftheuser,thepositionofthesatellite(basestation)andthereferencesatellite.TherangemeasurementsareandthedifferencemeasurementsTheproblemnowistosolvetheconditionalprobabilitydensityfunction(cpdf)oftheposteriorstate,thatisthecpdfofconditionedon.UsingtheBayesrulewiththeassumptionofwhitenoisesequencesthecpdfmaybeformulatedas isthepriordensity,i.e.thecpdfofconditionedonisthelikelihoodfunctiongivingtheprobabilityofmeasurementgivenstateisthepredictedmeasurementdensity.Toderivestatisticalquantitieslikemeanandcovariancefromcpdf(5)requiresintegrationofnumerousmultidimensionalintegralsusingnumericalintegrationmethods.GridbasedmethodsandsequentialMonteCarlo(particle“lter)methodshavebeenusedsuccesfullytocalculatetheposteriorestimates.Theyhoweverrequirealotofcomputationandarenotsuitableinrealtimepositioningwithtodaysmobiledevices.Inthenextsection,oneapproximateanalyticsolution,namelytheextendedKalman“lter,isintroduced.A.ExtendedKalmanFilterTheextendedKalman“lterisanapproximateanalyticsolutionto(5)ifthenoisesareadditivegaussiannoisesequences.Thestateandmeasurementfunctionsarelinearizedaccordingto xkk,Hk=hk(xk) arethepriorandposteriormeanestimatesattimestep.ThesystemmodelbecomesNowthepriordensityisgaussianwithmeanandcovariancegivenas)=isthecovarianceof.Theposteriormeanmaybeshowntobe)=.Denotingthecovarianceoftheposteriormeanbecomesandtheposteriorcovarianceisgivenby=(IistheKalmangainisassumedtobepositivede“nitetomakesurethattheinverseexists.III.CLASSESOFAsseenintheprevioussection,Kalman“lteringisbasedontheassumptionofgaussianmeasurementnoiseandgaussianpriordistribution.Asarguedbefore,thestrictassumptionofgaussianmeasurementnoisemaynotbereasonableinhybridpositioningapplicationsandthereforesomeŽmorerobustŽdensitiesarepresentedhere.Huber[2]proposedagamewhereNaturechosesthedis-tributionandtheengineerchoosestheestimator,andtheasymptoticvarianceT,Fisthepay-offtotheengineer.Theasymptoticvarianceofanestimatoristhevarianceoftheestimatorwhenthesamplesizetendstoin“nity.Itcanbeshownthatforcertainclassesofdistributionsthereexistsamin-maxsolutionsuchthatT,F)=V()=maxT,Fiscalledtheleastfavorabledistributionofclassthemin-maxrobustestimatorwhichisactuallythemaximumlikelihoodestimatorfortheleastfavorabledensity A.The-contaminatednormalneighborhoodHuber[2]consideredthecasewhereisthecontaminatednormalneighborhood=(1H,continuoussymmetricalpdfisthestandardnormalprobabilitydensityandisacontinuoussymmetricalprobabilitydensityfunction.Theleastfavorabledensityforthe-contaminatednormalneighborhood  2eŠ1 22,||k)  2e1 wherethethresholdparameterisgivenby kŠk isthestandardnormalpdfandthestandardnormalcdf.Thecorrespondingmin-maxrobustestimatorisbasedonthemaximizationofthelikelihoodscoreoftheleastfavorabledensity,namely FiltersusingthisscorefunctionarelabelledŽHŽ.B.The-pointfamilyMartinandMasreliez[3]assertthatifisthefamily2=(symmetricandcontinuousatisthestandardnormalcumulativedistributionfunc-tion,thenthecorrespondingleastfavorabledensityisgiven 2),||ypK(1 2m)e21(1 =(1(1+ isgivenby1+tan  +tan Thelikelihoodscoreoftheleastfavorabledensityofthepointfamily p0p(Š1 2),||ypŠ1 )sign(FiltersusingthisscorefunctionarelabelledŽMŽ.C.EstimatorswithoutdensitiesThemin-maxrobustestimatorsdependontheshapeoftheleastfavorabledensity.However,inmoresophisticatedrobust“lterdesign,itmightbemoreappropriatetomodelthescorefunctiontocorrespondtothespeci“cprobleminsteadof“ndingtheappropriatecontaminationclassesandtheleastfavorabledensities.Andrewset.al[4]proposeathree-parts-redescendingestimatorwhichisbasicallyaheuristicmodi“ca-tionoftheestimatorproposedbyHuber.Becauseredescendingestimatorsrejectcertainmeasurementsamodi“edversionofthethree-parts-redescendingestimatorisproposedhere.Thescorefunctionisgivenby wherethethresholdparametersarechosento“ttheproblem.Inthisworkissolvedasin(16)andFiltersusingthisscorefunctionarelabelledŽDŽ.IV.ROBUSTILTERINGIntheprevioussectionwehavede“nedsomeŽrobustŽdensitiesandtheircorrespondingscores.InthissectionweproceedtodescribetwodifferentwaysthatthesecouldbeusedtorobustifytheextendedKalman“lter.A.Weightedleastsquares“lteringThe“rstapproachisbasedontherobustKalman“lteringstudiesofCarosioet.al[5].TheKalman“ltercanbeshowntobeequivalenttoadeterministicleastsquaresproblem[6].The“lterismademorerobustbyreplacingtheleastsquaresscorefunctionbythescorefunctionsintroducedintheprevioussection.Byapproximatingthederivativeofthescorefunctionwithalinearapproximationtheproblemismodi“edtoaweightedleastsquaresproblem,wheretheweightsarecalculatedusingthecomponentsoftheinnovationvectorandaweightingfunctionthatcorrespondstotheselectedscorefunction.Thisapproachproduces“ltersthatgivelessin”uencetomeasurementsthatareclassi“edasblunder.ThestateupdateequationandthemeasurementequationoftheEKFgivenin(8)maybewrittenasTomaketheequationslookmoresimplede“neNow)=E(aresymmetricpositivede“niteandthusnon-singular.Nowde“nesquarematrixsuchthat Multiplying(23)byandde“ningwhichisintheformofastandardlinearleastsquaresregressionproblemorequivalently=argministhethrowof.Now)=E(,andthesolutionto(28)isgivenby=(Nwhichwillbetheposteriormean.Theposteriorcovarianceischosentobe=(N=(IiscalledtheKalmangainmatrix.Equations(29)and(30)areofcoursesimilartoequations(12)and(13).Huber[2]introducedaclassofestimators,calledM-estimators,thatminimizeotherfunctionalsthanthesquares.denotethefunctionalwhichwillbeminimized.Nowin(28)inplaceofgives=argminTheminimumisobtainedbydifferentiatingthefunctionaltobeminimizedwithrespecttoandsettingthesepartialderivativesequaltozero.Symbolicallythiscanwrittenasisthethelementofmatrix.Thesolutionof(32)issomewhatdif“culttoobtainatleastanalyticallybecausethe-functionsmaybenon-linear.However,(32)canbeapproximatedwithaweightedleastsquaresproblem,namelyk,iwheretheweightsk,iaregivenbyk,i =(N=(NNowthesolutionof(33)isgivenby=(Nwithcovariance=(I=diag(wheretheweights.Themotivationfordividingtheweightmatrixintotwosubmatricesisthatthein”uencesonlythestateupdateandthestatecorrectionwithmeasurements.Soifrobustnessisdesiredonlyinthemeasurementmodelonemightwantto.Dependingontheapplicationitmightbereasonabletousedifferent-functionfordifferentcomponentsin(32).Inthispaper,wherethechoicesforthelikelihoodscorearegiveninequations(17),(21)and(22).Figure1showsthe-functionsandthecorrespondingweightfunctions.The“ltersusingthismethodaredenotedwithpre“xB.Bayesian“lteringassumingnon-gaussianinnovationse-ThefollowingmethodisbasedontheresultsofMasreliezandMartin[7].Consideratransformedversionofthemea-surementmodelin(8).Thetransformationmatrix=(Hchosensuchthat=(H,whichiseasilyobtainedforexampleusingthesingularvaluedecomposition.Thusthetransformedinnovationvariablehasastandardnor-maldensitydistribution.TheKalman“ltercalculatestheposteriorstateestimatesusingtheinnovationvariable.Underthetransformedmeasure-mentmodel(38)theposteriormeanisgivenbywherethescoreisthetransformedinnovation.MasreliezandMartinassertthatifistheleastfavorabledensityofeitherthentheposteriormeanisgivenasin(39)andtheposteriorcovarianceisgivenby=(I,thenandif,then(1+tan The“ltersusingthismethodaredenotedwithpre“xŽBŽ V.TTherobustmethodswereimplementedinaMATLABlationtestbenchforcomparisonbetweendifferent“lters.Thesimulationtestbenchwasdesignedtoproducedynamictestdatasimilartowhatcouldbeexpectedinreal-worldpersonalpositioningscenario.Themaindifferencefromtherealdataisthatinthesimulationthetruetrackandcorrectmeasurementandmotionmodelsareavailable.Thetestingprocessconsistsof“rstgeneratingatruetrackof100pointsatonesecondintervalswithavelocity-restrictedrandomwalkmodel,thengeneratingasetofbasestation(BS)alongthetrackwithmaximumrangessetsothatonetothreestationscanbeheardfromeverypointonthetrack.AGPSconstellationisthensimulatedwithanelevationmaskandshadowingpro“lesetsothatonlyacoupleofsatellitesarevisibleatatime.Finally,noisymeasurementsaregeneratedforeachtimestepfromthevisiblesatelliterangesanddelta-ranges,andbasestationsrangesandaltitudemeasurements.Ifprobabilityforblunderisgreaterthanzero,theneverygeneratedmeasurementfailswithaprobabilityofSeveraltrackandmeasurementsetsweregeneratedwithdif-ferentmeasurementsources,measurementnoisesandblundermeasurementprobabilities.Theresultingtestbenchconsistsoftwodifferentsetsaccordingtotwodifferentchoicesfortheblundermeasurementprobability(0.00and0.05).Eachsethasbasestationandsatellitegeometriesfor9casesrepresentingdifferentdif“cultpositioningsituationsaccordingtothefollowingtable mean#BS mean#GPS Case1 Case2 Case3 Case4 Case5 Case6 Case7 0 Case8 0 Case9 0 Everycasecontaines100tracks,whichallhavethelengthof100timesteps.Thetesttrackswere“lteredwiththesixrobust“ltersofthispaper,andthemeanandcovarianceoftheposteriordistributionrecordedateachtimestep.The“lteredsolutionswerecomparedtothetruetrackandsomestatisticsderived.The“ltersabilitytoidentifyblundermeasurementswasalsotestedandreported.Forcomparison,thedatawasalsoprocessedwithEKFandEKF2[1].A.ResultsTheresultsofthetestbenchmaybeseenintablesIandII.Thequantitiescalculatedarethemean2Derror(MSE)giveninmeters,theboundbelowwhichare95%oftheerrors,andthefrequencyofinconsistentestimates.AFilterissaidtobeinconsistentiftheerrorestimateissmallerthantheactualerror.FordiscussionontheconsistencyofEKFandEKF2see[1].AsexpectedalltheB-“ltersoutperformEKFandEKF2incontaminatedcases.Itisinterestingtonoticehowever,thatwhenthereareGPSmeasurementsavailabletheWLS-“ltersfailcompeletely.InbasestationonlycasestheWLS-solverssometimesoutperformeventheB-“lters.TheWLS“ltersfailureseemstobecausedbythelargeinnovationsthatsometimesoccurwithGPS-measurements.Thesolversmayeasilygiveaweightoflessthan0.01forsomemeasurements.The“ltersabilitytodetectthecorrectcorruptedmeasurementwasalsoquitepoor.Onaveragethe“ltersidentifywrongmeasurementmoreoftenthantherightmeasurement,andthisbecomesaseriousproblemwiththeWLS-“lters,becausecorrectmeasurementsgetscaledclosetozero.Allthatcanbesaidabouttheblunderdetectionis,thatitisextremelyhardtotellifameasurementisablundermeasurementbymeasuringthemagnitudeofthecorrespondinginnovation-thepriormightalsobewrong,butthe“lterindenti“esthemeasurementsasblunder.IntablesIIIandIVarelistedhowwellthe“ltersdetectandidentifyblundermeasurements.Theactionsofthe“ltermaybedividedinto“vecategories:IBlundermeasurementsarepresentandthe“lterdetectsthemcorrectly.Filtercalculatesrobustestimates.IIBlundermeasurementsarepresentbutthe“lteridenti“eswrongmeasurement.IIIBlundermeasurementsarepresentbutthe“lterdoesnotnoticeit,thusworkingasEKF.IVFalsealarm.Noblundermeasurements,but“ltercalcu-latesrobustestimates.VNoblundermeasurementsandnoaction.Thismeansthatthe“lters,exceptthe“ltersbasedonthe-pointscore,workasEKF.Thefrequenciesofcategories1-4arelistedinthetableandthecategoriesaredenotedbythecorrespondingRomannumeral.Thefrequencyofcategory5isthefrequencyofthecomplementoftheunionofthethree“rstcategories.TheFalsealarmsoccurquiteoften,buttheyarenotverydangerousexceptforWLS-“lterswithGPS-measurements.Iftherewereblundermeasurementspresent,the“ltersdetectedthemalmosteverytime.Themostinterestingthinghoweveristhehugefrequencyofcategory2inthecontaminatedcase.Becausethefalsealarmistheonlyonefromthecategories1-4whichcanhappenintheuncontaminatedcase,thezerofrequenciesareleftoutfromtableIII.VI.CBasedonthesimulations,theproposedmethodsseemtooutperformEKFandEKF2incontaminatedcasesanddoalmostaswellinnormalcases.Therefore,theproposedmeth-odsshouldbetakenintoconsiderationtobeusedinmobilepositioningdevices.Theapproximatebayesian“lteristobepreferredovertheWLS-“lterifthereareGPS-measurementsavailable.WLS-“ltersmightbeconsideredotherwise.Thescorefunctionsshouldbechosenbasedonempiricalmea-surementdata,butwithoutanypreliminaryknowledgeofthesituationitseemstobesafetouseanyofthescoresproposedhere.However,themodi“edHampelestimatorseemstogive bestestimates.Moresophisticatedrobust“ltershouldbeabletoindentifycertainsituationsandapplythemostconvenientrobustmodeltothem,butthisisbeyondthescopeofthispaperandleftforfuturestudy.CKNOWLEDGMENTThisstudywasfundedbyNokiaCorporation.TheEKFandEKF2usedforreferencewereimplementedbySimoAli-oyttyandthesimulationtestbenchwasgeneratedusingNiiloSirolastestbenchgenerator[8].[1]S.Ali-Loytty,N.Sirola,andR.Piche,ConsistencyofThreeKalmanFilterExtensionsinHybridNavigation,ŽinProceedingsoftheEuropeanNavigationConferenceGNSS,July19-222005.[2]P.J.Huber,RobustEstimationofaLocationParameter,ŽAnn.Math.,vol.35,pp.73…101,1964.[3]R.D.MartinandC.J.Masreliez,RobustEstimationviaStochasticApproximation,ŽIEEETransactionsonInformationTheory,vol.IT-21,no.3,pp.263…271,May1975.[4]D.F.Andrews,P.J.Bickel,F.R.Hampel,P.J.Huber,W.H.Rogers,andJ.W.Tukey,RobustEstimatesofLocation:SurveyandAdvancesPrincetonUniversityPress,1972.[5]A.Carosio,A.Cina,andM.Piras,TheRobustStatisticsMethodAppliedtotheKalmanFilter:TheoryandApplication,ŽIONGNSS18thInternationalTechnicalMeetingoftheSatelliteDivision,September13-162005.[6]A.E.BrysonandY.-C.Ho,AppliedOptimalControl:Optimization,Estimation,andControl.Taylor&Francis,1975.[7]C.J.MasreliezandR.D.Martin,RobustBayesianEstimationfortheLinearModelandRobustifyingtheKalmanFilter,ŽIEEETransactionsonAutomaticControl,vol.AC…22,no.3,pp.361…371,1977.[8]N.Sirola,S.Ali-Loytty,andR.Piche,Benchmarkingnonlinear“lters,ŽNonlinearStatisticalSignalProcessingWorkshopNSSPW06,Cam-bridge,September2006. Fig.1.The-andthe kŠkkktH(t) 1k1Šk1Šk1k2Šk2tD(t) yŠyyytM(t) kkH(t)t1 1Šk1k2Šk2t1D(t) yyM(t)t14th WORKSHOP ON POSITIONING, NAVIGATION AND COMMUNICATION 2007 (WPNC07), HANNOVER, GERMANY61 TABLEIESULTSOFTHETESTBENCHWITHBLUNDERPROB Case1 Case2 Case3 =0% MSE95%incon MSE95%incon MSE95%incon 12949014.7 401383.8 11290.0 1234533.8 451722.2 11290.0 13853415.3 471684.8 12300.0 13551015.0 431564.5 12300.0 13251814.8 441524.3 12300.0 14453216.0 431554.1 12310.0 13249314.9 391373.4 12300.0 13350515.3 401413.8 12300.1 Case4 Case5 Case6 MSE95%incon MSE95%incon MSE95%incon 13330.1 9200.0 20680.8 13330.0 9200.0 19640.3 13330.1 9200.0 23761.4 13340.1 9200.0 22751.4 13500.4 9200.0 21701.2 13340.1 9200.0 21710.9 13330.1 9200.0 21701.0 13560.2 9200.0 21701.0 Case7 Case8 Case9 MSE95%incon MSE95%incon MSE95%incon 18410.0 19430.0 17370.0 18410.0 19430.0 17370.0 19420.0 19440.0 17380.0 19420.0 19440.0 17380.0 19420.0 19440.0 17380.0 19420.0 19430.0 17370.0 19420.0 19430.0 17370.0 19420.0 19430.0 17380.0 TABLEIIESULTSOFTHETESTBENCHWITHBLUNDERPROB Case1 Case2 Case3 =5% MSE95%incons. MSE95%incons. MSE95%incons. 351146642.0 16869338.6 8532835.2 329131737.6 16366735.3 8432635.0 16265615.5 894169.7 14350.6 17766718.5 9143311.2 15401.1 19380421.1 8840312.4 16421.5 17072516.4 904379.0 12300.0 16966717.7 834149.3 14340.2 17165318.6 793859.5 14360.2 Case4 Case5 Case6 MSE95%incons. MSE95%incons. MSE95%incons. 503149186.5 317103379.2 372124871.6 502149186.5 317103379.2 370124271.1 17420.1 12290.5 21600.9 20510.1 14361.6 32732.6 21531.6 15392.4 34823.8 23971068986.0 21501028378.2 40901933572.7 855141384.1 548199675.6 737287768.7 762235184.2 487181575.2 696257468.2 Case7 Case8 Case9 MSE95%incons. MSE95%incons. MSE95%incons. 1020306592.3 989270593.8 949254895.5 1020306592.3 989270593.8 949254895.5 25590.3 23490.0 21450.0 28680.4 26560.3 26571.0 30730.9 27600.8 28612.1 70902513890.5 67872602492.5 38391512394.2 2351792690.7 2013625593.0 1734553094.8 2283761790.4 1873603293.1 1615493694.9 4th WORKSHOP ON POSITIONING, NAVIGATION AND COMMUNICATION 2007 (WPNC07), HANNOVER, GERMANY62 TABLEIIILUNDERDETECTIONWITHBLUNDERPROBABILITY Case1Case2Case3 Case4Case5Case6 Case7Case8Case9=0% IIIIIIIII IIIIIIIII IIIIIIIII 0.210.280.37 0.450.550.33 0.300.410.59 0.210.270.37 0.450.550.33 0.300.410.59 0.220.290.39 0.460.560.34 0.300.410.59 0.380.430.52 0.580.660.47 0.460.570.70 0.370.420.51 0.580.660.47 0.460.570.70 0.370.420.51 0.580.660.47 0.470.570.70TABLEIVLUNDERDETECTIONWITHBLUNDERPROBABILITY Case1 Case2 Case3=5% IIIIIIIV IIIIIIIV IIIIIIIV 0.010.030.020.25 0.020.040.020.30 0.030.060.030.41 0.010.030.010.27 0.020.040.020.32 0.030.060.020.44 0.010.030.010.29 0.020.040.020.34 0.030.060.020.45 0.020.020.010.40 0.030.030.010.42 0.030.060.020.50 0.020.020.010.42 0.030.030.010.44 0.030.060.020.52 0.020.020.010.42 0.030.030.010.44 0.030.060.020.52 Case4 Case5 Case6 IIIIIIIV IIIIIIIV IIIIIIIV 0.070.110.090.45 0.090.160.090.47 0.050.080.090.39 0.080.110.090.46 0.090.170.080.48 0.050.080.080.41 0.080.110.090.47 0.090.170.080.49 0.050.080.080.42 0.220.050.000.70 0.250.080.010.65 0.150.040.010.69 0.220.050.010.69 0.240.080.010.64 0.150.050.010.68 0.220.060.000.70 0.230.090.010.64 0.150.050.010.67 Case7 Case8 Case9 IIIIIIIV IIIIIIIV IIIIIIIV 0.080.140.030.27 0.100.190.020.31 0.160.250.010.37 0.080.140.020.29 0.110.180.020.33 0.160.250.010.39 0.080.140.020.30 0.100.190.020.34 0.160.250.010.40 0.240.010.000.72 0.300.010.000.67 0.410.010.000.57 0.240.010.000.72 0.300.010.000.67 0.410.010.000.57 0.240.010.000.71 0.300.010.000.67 0.410.010.000.57