USF Libraries
USF Digital Collections

Accurate localization given uncertain sensors

MISSING IMAGE

Material Information

Title:
Accurate localization given uncertain sensors
Physical Description:
Book
Language:
English
Creator:
Kramer, Jeffrey
Publisher:
University of South Florida
Place of Publication:
Tampa, Fla
Publication Date:

Subjects

Subjects / Keywords:
Sensor fusion
Mathematical filters
Robot localization
Sigma-point kalman filter
Particle filter
Dissertations, Academic -- Computer Science and Engineering -- Masters -- USF   ( lcsh )
Genre:
non-fiction   ( marcgt )

Notes

Abstract:
ABSTRACT: The necessity of accurate localization in mobile robotics is obvious - if a robot does not know where it is, it cannot navigate accurately to reach goal locations. Robots learn about their environment via sensors. Small robots require small, efficient, and, if they are to be deployed in large numbers, inexpensive sensors. The sensors used by robots to perceive the world are inherently inaccurate, providing noisy, erroneous data or even no data at all. Combined with estimation error due to imperfect modeling of the robot, there are many obstacles to successfully localizing in the world. Sensor fusion is used to overcome these difficulties - combining the available sensor data in order to derive a more accurate pose estimation for the robot. In this thesis, we dissect and analyze a wide variety of sensor fusion algorithms, with the goal of using a set of inexpensive sensors in a suite to provide real-time localization for a robot given unknown sensor errors and malfunctions. The sensor fusion algorithms will fuse GPS, INS, compass and control inputs into a more accurate position. The filters discussed include a SPKF-PF (Sigma-Point Kalman Filter - Particle Filter), a MHSPKF (Multi-hypothesis Sigma-Point Kalman Filter), a FSPKF (Fuzzy Sigma-Point Kalman Filter), a DFSPKF (Double Fuzzy Sigma-Point Kalman Filter), an EKF (Extended Kalman Filter), a MHEKF (Multi-hypothesis Extended Kalman Filter), a FEKF (Fuzzy Extended Kalman Filter), and a standard SIS PF (Sequential Importance Sampling Particle Filter). Our goal in this thesis is to provide a toolbox of algorithms for a researcher, presented in a concise manner. I will also simultaneously provide a solution to a difficult sensor fusion problem - an algorithm that is of low computational complexity (< O(n^3)), real-time, accurate (equal in or more accurate than a DGPS (differential GPS) given lower quality sensors), and robust - able to provide a useful localization solution even when sensors are faulty or inaccurate. The goal is to find a locus between power requirements, computational complexity and chip requirements and accuracy/robustness that provides the best of breed for small robots with inaccurate sensors. While other fusion algorithms work well, the Sigma Point Kalman filter solves this problem best, providing accurate localization and fast response, while the Fuzzy EKF is a close second in the shorter sample with less error, and the Sigma-Point Kalman Particle Filter does very well in a longer example with more error. Fuzzy control is also discussed, especially the reason for its applicability and its use in sensor fusion.
Thesis:
Thesis (M.S.C.S.)--University of South Florida, 2010.
Bibliography:
Includes bibliographical references.
System Details:
Mode of access: World Wide Web.
System Details:
System requirements: World Wide Web browser and PDF reader.
Statement of Responsibility:
by Jeffrey Kramer.
General Note:
Title from PDF of title page.
General Note:
Document formatted into pages; contains X pages.

Record Information

Source Institution:
University of South Florida Library
Holding Location:
University of South Florida
Rights Management:
All applicable rights reserved by the source institution and holding location.
Resource Identifier:
usfldc doi - E14-SFE0003465
usfldc handle - e14.3465
System ID:
SFS0027780:00001


This item is only available as the following downloads:


Full Text
xml version 1.0 encoding UTF-8 standalone no
record xmlns http:www.loc.govMARC21slim xmlns:xsi http:www.w3.org2001XMLSchema-instance xsi:schemaLocation http:www.loc.govstandardsmarcxmlschemaMARC21slim.xsd
leader nam 22 Ka 4500
controlfield tag 007 cr-bnu---uuuuu
008 s2010 flu s 000 0 eng d
datafield ind1 8 ind2 024
subfield code a E14-SFE0003465
035
(OCoLC)
040
FHM
c FHM
049
FHMM
090
XX9999 (Online)
1 100
Kramer, Jeffrey.
0 245
Accurate localization given uncertain sensors
h [electronic resource] /
by Jeffrey Kramer.
260
[Tampa, Fla] :
b University of South Florida,
2010.
500
Title from PDF of title page.
Document formatted into pages; contains X pages.
502
Thesis (M.S.C.S.)--University of South Florida, 2010.
504
Includes bibliographical references.
516
Text (Electronic thesis) in PDF format.
538
Mode of access: World Wide Web.
System requirements: World Wide Web browser and PDF reader.
3 520
ABSTRACT: The necessity of accurate localization in mobile robotics is obvious if a robot does not know where it is, it cannot navigate accurately to reach goal locations. Robots learn about their environment via sensors. Small robots require small, efficient, and, if they are to be deployed in large numbers, inexpensive sensors. The sensors used by robots to perceive the world are inherently inaccurate, providing noisy, erroneous data or even no data at all. Combined with estimation error due to imperfect modeling of the robot, there are many obstacles to successfully localizing in the world. Sensor fusion is used to overcome these difficulties combining the available sensor data in order to derive a more accurate pose estimation for the robot. In this thesis, we dissect and analyze a wide variety of sensor fusion algorithms, with the goal of using a set of inexpensive sensors in a suite to provide real-time localization for a robot given unknown sensor errors and malfunctions. The sensor fusion algorithms will fuse GPS, INS, compass and control inputs into a more accurate position. The filters discussed include a SPKF-PF (Sigma-Point Kalman Filter Particle Filter), a MHSPKF (Multi-hypothesis Sigma-Point Kalman Filter), a FSPKF (Fuzzy Sigma-Point Kalman Filter), a DFSPKF (Double Fuzzy Sigma-Point Kalman Filter), an EKF (Extended Kalman Filter), a MHEKF (Multi-hypothesis Extended Kalman Filter), a FEKF (Fuzzy Extended Kalman Filter), and a standard SIS PF (Sequential Importance Sampling Particle Filter). Our goal in this thesis is to provide a toolbox of algorithms for a researcher, presented in a concise manner. I will also simultaneously provide a solution to a difficult sensor fusion problem an algorithm that is of low computational complexity (< O(n^3)), real-time, accurate (equal in or more accurate than a DGPS (differential GPS) given lower quality sensors), and robust able to provide a useful localization solution even when sensors are faulty or inaccurate. The goal is to find a locus between power requirements, computational complexity and chip requirements and accuracy/robustness that provides the best of breed for small robots with inaccurate sensors. While other fusion algorithms work well, the Sigma Point Kalman filter solves this problem best, providing accurate localization and fast response, while the Fuzzy EKF is a close second in the shorter sample with less error, and the Sigma-Point Kalman Particle Filter does very well in a longer example with more error. Fuzzy control is also discussed, especially the reason for its applicability and its use in sensor fusion.
590
Advisor: Abraham Kandel, Ph.D.
653
Sensor fusion
Mathematical filters
Robot localization
Sigma-point kalman filter
Particle filter
690
Dissertations, Academic
z USF
x Computer Science and Engineering
Masters.
773
t USF Electronic Theses and Dissertations.
4 856
u http://digital.lib.usf.edu/?e14.3465



PAGE 1

AccurateLocalizationGivenUncertainSensorsbyJeffreyA.KramerAthesissubmittedinpartialfulllmentoftherequirementsforthedegreeofMasterofScienceinComputerScienceDepartmentofComputerScienceandEngineeringCollegeofEngineeringUniversityofSouthFloridaMajorProfessor:AbrahamKandel,Ph.D.SrinivasKatkoori,Ph.D.DeweyRundus,Ph.D.AdrianaIamnitchi,Ph.D.JeffCraighead,Ph.D.DateofApproval:April8,2010Keywords:sensorfusion,mathematicallters,robotlocalization,sigma-pointkalmanlter,particlelter cCopyright2010,JeffreyA.Kramer

PAGE 2

Dedication Tomyfamily,forwithoutyou,Icouldneverhavemadeitthisfar.

PAGE 3

Acknowledgments IwouldliketothankDr.RobinMurphyforhersupportandassistancethroughtherstcrucialstepsofthisthesisandfordealingwiththeunfocusedstumblingsittookonitswaytoatruepaper.IwouldliketoalsothankDr.KimonValavanisforinvaluableeditingandadvice-Iwouldnothavelastedthesummerof'08withouthishelp.Finally,IwouldliketothankDr.AbrahamKandelforhisunwaveringsupport,incisiveadvice,andfortakingmeinafterIlosttwomajorprofessorsinasmanymonths.ThankyouDr.Kandel-Ilookforwardtoworkingwithyouintothefuture.

PAGE 4

TableofContentsListofTablesivListofFiguresviAbstractviiiChapter1Introduction11.1ProblemStatement 2 1.2ProposedSolution 2 1.3SummaryofResults 3 1.4Contributions 4 1.5ThesisOutline 4 Chapter2LiteratureReview52.1Non-AdaptiveFilters 5 2.1.1KalmanFiltersandDerivatives 5 2.1.1.1ExtendedKalmanFilters 6 2.1.1.2ExtendedInformationFilters 6 2.1.1.3UnscentedKalmanFilters 6 2.1.2KalmanFiltersforRobotLocalization 7 2.1.2.1SLAMandKalmanFilterLocalization 8 2.1.2.2SPKFforRobotLocalization 8 2.2AdaptiveFilters 9 2.2.1InherentAdaptation-ParticleFilters 9 2.2.1.1ParticleFiltersandRobotLocalization 10 2.2.1.2ParticleFiltersandSLAM 12 2.2.2InherentAdaptation-MultipleHypothesisEKF 13 2.2.2.1MultipleHypothesisEKFandRobotLocalization 13 2.2.3DrivenAdaptation-Innovation-BasedAdaptiveEstimation 14 2.2.3.1Innovation-BasedAdaptiveEstimationandSensorFusion 14 2.2.3.2Innovation-BasedAdaptiveEstimationandRobotLocalization 15 2.2.3.3DrivenParticleFilter 15 2.2.4FuzzyAdaptiveFilters 16 2.3Conclusions 16 Chapter3ComparativeAnalysisofLocalizationAlgorithms193.1Introduction 19 3.2KalmanFiltersandDerivatives 20 3.2.1EKF 20 3.2.2EIF 20 3.2.3SPKF 21 3.3AdaptableFilters 22 3.3.1InherentAdaptation-ParticleFilters 23 i

PAGE 5

3.3.2InherentAdaptation-MultipleHypothesisEKF/SPKF 23 3.3.3DrivenAdaptation-Innovation-BasedAdaptiveEstimation 24 3.4Conclusions 24 Chapter4SensorFilterImplementation274.1Introduction 27 4.2KalmanFiltersandDerivatives 27 4.2.1ExtendedKalmanFilter 27 4.2.2EKFEquations 29 4.2.2.1Divergence 30 4.2.3SPKF 31 4.3ParticleFilters 34 4.3.1DensityExtraction 36 4.3.2ParticleDeprivationandDegeneracy 36 4.3.3ResamplingandSamplingVariance 39 4.3.4SamplingBias 40 4.3.5KidnappedRobotProblem 40 4.3.6ChoosingtheRightNumberofParticles 41 4.3.7MultipleHypothesisEKF/SPKF 43 4.4DrivenAdaptation 44 4.5FuzzyControl 48 4.5.1Introduction 48 4.5.2FuzzySystemsandUncertainty 49 4.5.3FuzzyControllers 50 4.5.3.1DefuzzicationMethods 51 4.5.4FuzzyOptimality 54 4.6Conclusions 55 Chapter5Results565.1ExperimentalDesign 56 5.2ExtendedKalmanFilter 57 5.2.1Design 57 5.2.2Results 57 5.3FuzzyEKF 61 5.3.1Design 61 5.3.2Results 61 5.4Sigma-PointKalmanFilter 62 5.4.1Design 62 5.4.2Results 62 5.5FuzzySPKF 64 5.5.1Design 64 5.5.2Results 65 5.6DoubleFuzzySPKF 65 5.6.1Design 65 5.6.2Results 66 5.7SISParticleFilter 69 5.7.1Design 69 5.7.2Results 69 5.8SPKFParticleFilter 70 5.8.1Design 70 5.8.2Results 70 ii

PAGE 6

5.9MultipleHypothesisMethods 72 5.10Comparisons 77 Chapter6Conclusions826.1Summary 82 6.2FutureWork 83 References84Bibliography90Appendices94AppendixA:MathematicalFilterCode 95 AppendixB:RobotSimulationCode 264 AppendixC:RobotStatisticsCode 283 iii

PAGE 7

ListofTablesTable1.StandardDatasetResults 3 Table2.BridgeDatasetResults 3 Table3.20PointDatasetResults 4 Table4.KalmanFilterAnalysis 20 Table5.ExtendedKalmanFilterAnalysis 21 Table6.ExtendedInformationFilterAnalysis 21 Table7.SigmaPointKalmanFilterAnalysis 22 Table8.ParticleFilterAnalysis 23 Table9.MultipleHypothesisExtended/SigmaPointKalmanFilterAnalysis 24 Table10.TraditionalInnovation-BasedAdaptiveEstimationAnalysis 25 Table11.FuzzyInnovation-BasedAdaptiveEstimationAnalysis 25 Table12.SPKFVariables 31 Table13.ResultsforEKF 57 Table14.ResultsforFEKF 61 Table15.ResultsforSPKF 62 Table16.ResultsforFSPKF 65 Table17.FuzzyControllerDesignforDoubleFuzzySPKF-Controllingand 66 Table18.FuzzyControllerDesignforDoubleFuzzySPKF-ControllingRx 66 Table19.ResultsforDFSPKF 68 Table20.ResultsforSISPF 69 Table21.ResultsforSPKFPF 71 Table22.MultipleHypothesisResultsfortheStandardDataset 72 Table23.MultipleHypothesisResultsfortheBridgeDataset 72 iv

PAGE 8

Table24.MultipleHypothesisResultsforthe20PointDataset 72 Table25.StandardDatasetResults 77 Table26.BridgeDatasetResults 77 Table27.20PointDatasetResults 78 v

PAGE 9

ListofFiguresFigure1.EKF/SPKFSensorFusionMethod 28 Figure2.ParticleFilterSensorFusionMethod 37 Figure3.CovarianceMembershipFuzzyInput 46 Figure4.MeanMembershipFuzzyInput 46 Figure5.AlphaFuzzyOutput 47 Figure6.FuzzySurfaceoftheFLAC 47 Figure7.OverviewofaFuzzyController 52 Figure8.TheTrueRobotPathfortheStandardandBridgeDatasets 57 Figure9.TheGPSOutputfortheBridgeDataset 58 Figure10.TheTrueRobotPathforthe20PointDataset 58 Figure11.TheGPSOutputforthe20PointDataset 59 Figure12.TheKalmanCascadeDesign 59 Figure13.TheEKFOutputfortheStandardandBridgeDataset 60 Figure14.TheEKFOutputforthe20PointDataset 60 Figure15.TheFEKFOutputfortheStandardandBridgeDatasets 61 Figure16.TheFEKFOutputforthe20PointDataset 62 Figure17.TheSPKFOutputfortheStandardandBridgeDatasets 63 Figure18.TheSPKFOutputforthe20PointDataset 63 Figure19.FuzzyControllerforQandR 64 Figure20.TheFSPKFOutputfortheStandardandBridgeDatasets 65 Figure21.TheFSPKFOutputforthe20PointDataset 66 Figure22.DoubleFuzzyControllerforand 67 Figure23.DoubleFuzzyControllerforRx 67 vi

PAGE 10

Figure24.TheDFSPKFOutputfortheStandardandBridgeDatasets 68 Figure25.TheDFSPKFOutputforthe20PointDataset 68 Figure26.TheSISPFOutputfortheStandardandBridgeDatasets 69 Figure27.TheSISPFOutputforthe20PointDataset 70 Figure28.TheSPKFPFOutputfortheStandardandBridgeDatasets 71 Figure29.TheSPKFPFOutputforthe20PointDataset 71 Figure30.TheMHEKFResults 73 Figure31.TheMHFEKFResults 74 Figure32.TheMHSPKFResults 75 Figure33.TheMHFSPKFResults 76 Figure34.TheSPKFOutputfortheBridgeDataset 78 Figure35.TheFEKFOutputfortheBridgeDataset 79 Figure36.TheGPSOutputfortheBridgeDataset 79 Figure37.TheSPKFOutputforthe20PointDataset 80 Figure38.TheMHFEKFOutputforthe20PointDataset 80 Figure39.TheGPSOutputforthe20PointDataset 81 vii

PAGE 11

AccurateLocalizationGivenUncertainSensorsJeffreyA.KramerABSTRACT Thenecessityofaccuratelocalizationinmobileroboticsisobvious-ifarobotdoesnotknowwhereitis,itcannotnavigateaccuratelytoreachgoallocations.Robotslearnabouttheirenvironmentviasensors.Smallrobotsrequiresmall,efcient,and,iftheyaretobedeployedinlargenumbers,inexpensivesensors.Thesensorsusedbyrobotstoperceivetheworldareinherentlyinaccurate,providingnoisy,erroneousdataorevennodataatall.Combinedwithestimationerrorduetoimperfectmodelingoftherobot,therearemanyobstaclestosuccessfullylocalizingintheworld.Sensorfusionisusedtoovercomethesedifculties-combiningtheavailablesensordatainordertoderiveamoreaccurateposeestimationfortherobot.Inthisthesis,wedissectandanalyzeawidevarietyofsensorfusionalgorithms,withthegoalofusingasetofinexpensivesensorsinasuitetoprovidereal-timelocalizationforarobotgivenunknownsensorerrorsandmalfunctions.ThesensorfusionalgorithmswillfuseGPS,INS,compassandcontrolinputsintoamoreaccurateposition.TheltersdiscussedincludeaSPKF-PFSigma-PointKalmanFilter-ParticleFilter,aMHSPKFMulti-hypothesisSigma-PointKalmanFilter,aFSPKFFuzzySigma-PointKalmanFilter,aDFSPKFDoubleFuzzySigma-PointKalmanFilter,anEKFExtendedKalmanFilter,aMHEKFMulti-hypothesisExtendedKalmanFilter,aFEKFFuzzyExtendedKalmanFilter,andastandardSISPFSequentialImportanceSamplingParticleFilter.Ourgoalinthisthesisistoprovideatoolboxofalgorithmsforaresearcher,presentedinaconcisemanner.Iwillalsosimultaneouslyprovideasolutiontoadifcultsensorfusionproblem-analgorithmthatisoflowcomputationalcomplexity
PAGE 12

closesecondintheshortersamplewithlesserror,andtheSigma-PointKalmanParticleFilterdoesverywellinalongerexamplewithmoreerror.Fuzzycontrolisalsodiscussed,especiallythereasonforitsapplicabilityanditsuseinsensorfusion. ix

PAGE 13

Chapter1Introduction Localizationisthekeytomobilerobotics.Withouttheabilitytosuccessfullylocalizeitselfinanenvironment,arobotiseffectivelystrippedofitsabilitytodousefulwork.Sensorsarethetypicalmethodofperforminglocalization.Ifthesesensorswereperfect-neverfailednorreturnederroneousvalues-thelocalizationproblemwouldbesolved.However,thisisnotthecase.Sensorsusedtodayareckle,pronetoerrorsandnoisydata,aswellasfailures.Sensorsaretypicallycomplexmechanicalandelectronicdevicesthatmustcollapseaninnitenumberofvariablesintheactualworldintoasinglesetofvaluesfortherobottoconsume.Thistaskismademoredifcultbyoutsideinterference,suchasincidentsunlight,magneticelds,andmovinghumansandvehicles.Anindividualsensoroftentimesdoesnotprovideawholeoraccuratepicture.Proprioceptivesensorsregisterchangesintherobotfromtherobot'sperspective,whileexteroceptivesensorsrequireexternalstimulifortherobotGPSsatellites,roadsidemarkers,etc..Inordertodevelopacomplete,andideallyerror-freelocalization,onemustrelyonsensorfusion,ortheprocessofcombiningsensordata.Thesimplestmethodofsensorfusionwouldbeakintotakingtwohalvesofamapandmatchinguptheedges-justllinginforwhat'smissingfromtheothersensororsensors.However,thesimplemethodfailswhenonerealizesthatmapscouldbemisaligned,misprintedorjustwrong.Awayisneededtojudgethepotentialaccuracyoftheestimatedlocalization,theaccuracyofthesensors,andcometoabest-casebeliefofwheretherobotcurrentlyis,allinrealtime.Inordertoachievethisgoal,oneturnstomathematicallters,orrecursivemethodstocalculatebelief.Mathematicalltersprovidethebestrecoursetosolvingthisproblemasthey,ingeneral,recursivelysolvetheleast-squaresproblemofGauss-activelymovingtowardsamoreaccuratesolutionastheamountofresourcesspentontheproblemincrease.Thisself-correctingandhill-climbingbehaviorisextremelyusefulinsolvingthelocalizationproblem.Yourcurrentbestcaseisyourmostcurrentone,givenyourmostrecentsensormeasurements. 1

PAGE 14

1.1ProblemStatementThereisarecenturryofdiscussionaboutmobilemicrorobotsandtheiruseinbothmilitaryandciviliantasks.Thispresentsaverychallengingsetofproblemrequirements.Theserobotsareextremelysmall,typicallyweighingundervepoundsandtinthepalmofyourhand.Theyhaveextremelylimitedpayloadcapacity,power,andcomputationalresourcesavailable.Manyoftheserobotswillneedtheabilitytolocalizethemselves,necessitatingsensorsandalgorithmstfortheirparticularrequirements.Therearenocomparisonstudiesrelatingthedifferentpossibilitiesforlocalizationthatcomparestheirabilitiesandoffersconcreterecommendationsforfutureresearchers/robotdesigns.Afeelingofready-re-aimpervadesthediscipline-aresearcherdevelopsanewalgorithm,testsitagainstafewofthefrontrunnersanddeclaresvictory,withouteverdiscussingwhattypesofapplicationstheiralgorithmisappropriatefororpotentialproblemsthatmayarise.Ourgoalinthisthesisistoprovideanalgorithmthatisoflowcomputationalcomplexity
PAGE 15

1.3SummaryofResultsTheSPKFoutperformedalloftheothersensors-theSPKFwastheclearwinnerinallthedatasets.TheFuzzyEKFperformedataclosesecondinthesmallerStandardandBridgedatasets,exceptingspeed,whiletheSPKFPFandMHFEKFperformedexceptionallywellinthefarlonger20Pointdataset.TheSPKFprovidedasuperiorchoiceinthelocusappropriateforsmallrobotswithinaccuratesensors. Table1.StandardDatasetResults FilterType TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters GPS 100000 1.448 1.432 1.254 1.242 EKF 286.2 1.303 1.291 1.066 1.059 FEKF 160024.6 1.386 1.414 0.765 0.759 SPKF 14681.6 0.816 0.802 0.671 0.662 FSPKF 137346.3 0.637 0.624 0.943 0.928 DFSPKF 211777 0.667 0.651 1.077 1.068 SISPF 201572.3 2.373 2.281 3.389 3.163 SPKFPF 217061.7 2.431 2.343 3.400 3.201 MHEKF 1764 1.303 1.291 1.066 1.059 MHFEKF 803680.8 1.410 1.396 0.764 0.753 MHSPKF 72739 0.977 0.964 0.803 0.797 MHFSPKF 670522.5 1.277 1.263 1.086 1.079 Table2.BridgeDatasetResults FilterType TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters GPS 100000 2.703 2.784 1.839 1.901 EKF 289.8 2.360 2.443 1.564 1.617 FEKF 159957.2 1.973 2.052 1.111 1.150 SPKF 14534.6 1.519 1.596 0.986 1.024 FSPKF 137321.5 1.749 1.796 1.511 1.566 DFSPKF 211519.5 1.747 1.809 1.579 1.634 SISPF 207094.8 1.885 2.035 2.600 2.755 SPKFPF 204329.8 1.924 1.859 2.651 2.574 MHEKF 1764 2.360 2.443 1.564 1.617 MHFEKF 802855.8 1.987 2.048 1.114 1.147 MHSPKF 73889 1.802 1.899 1.180 1.223 MHFSPKF 669869.2 2.386 2.470 1.598 1.653 3

PAGE 16

Table3.20PointDatasetResults FilterType TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters GPS 3801860 7.189 6.758 2.667 2.655 EKF 10445.4 6.236 5.837 2.264 2.246 FEKF 4728402 7.200 6.771 2.668 2.656 SPKF 518997.5 4.070 3.780 1.435 1.418 FSPKF 5218453 6.375 5.970 2.320 2.303 DFSPKF 8017771 6.280 5.878 2.288 2.270 SISPF 7752004 7.848 5.026 7.229 3.550 SPKFPF 8022980 3.424 3.409 3.117 3.136 MHEKF 60501 6.236 5.837 2.264 2.246 MHFEKF 29825326 1.987 2.048 1.114 1.147 MHSPKF 2641112 4.070 3.780 1.485 1.438 1.4ContributionsInthisthesiswedescribethecurrentstateoftheartinrobotlocalizationwithafocusoncomparingdifferenttechniques,describingassumptionsinherentineach,aswellasthetradeoffsandbenets.Itdescribesallofthealgorithmsnecessaryinordertotestordevelopnewsensorfusionsuites.Italsoprovidesthemostappropriatechoiceofsensorfusionforasmallsensorsuite,giventherequirementslistedabove.Finally,thesoftwareusedtocompareallofthepossibleoptionsismadeavailableforfutureresearchers.1.5ThesisOutlineTheremainderofthisthesisisorganizedasfollows:Chapter2isareviewofthepre-existingliteratureandresearch,discussingallaspectsofmathematicalltersandsensorfusionandhowtheyrelatetorobotlocalization,aswellasdiscussingSLAMSimultaneousLocalizationandMapping;Chapter3discussestheproblemathandinmoredetailwithafocusonsmallsensorsaswellastheassumptions,tradeoffs,andbenetsassociatedwitheachtypeofsensorfusion;Chapter4discussestheproposedsolution,givinganin-depthdiscussionofthealgorithmsforeachsensorfusionmethod,aswellasfuzzycontrol;Chapter5discussestheresultsfromthetestsandcomparestheperformanceofallthedifferentsensortypes;nally,Chapter6drawsconclusionsabouttheappropriatenessofthenalsensorfusionmethod,givesanaloverviewofthepossiblechoicesforafutureresearcher,anddiscussespossiblefutureresearchavenues. 4

PAGE 17

Chapter2LiteratureReview Thestudyofhowtoextractusefulinformationfrompossiblyerroneousdatahasbeenunderwayforalongtime;theleastsquaresmethodofGauss,rstdescribedin1795,isthemethodthatalloftheltersofthistypederivetheirlineageSorenson1970.TheBayeslteristhemostgeneralrecursivemethodofcalculatingbelief,orthetheprobabilitythatapossiblehypothesisisthetruestate.TheBayeslter,alongwithallltersderivedfromit,relyontheMarkovassumption-thestatecontainsacompletesummaryofallpastdataorhistoryThrun,Burgard,andFox2005.TheBayeslteristheoptimallter,butitisimpossibletoimplementinacomputersystem,therefore,onemustexpandtheirexaminationtolookatltersthatcanbeiterativelyexecutedincomputers.TheexaminationofthetopicwillstartwiththeKalmanFilterKFKalman1960anditsvariants.Nextareadaptablelters-ltersthatpossessinherentadaptability,likeparticleltersPFandmulti-hypothesisExtendedKalmanFiltersMHEKF,aswellasthosethatpossessdrivenadaptability,likeInnovation-BasedAdaptiveEstimationIAEanddrivenParticleFilters.ThismirrorsthegradualdevelopmentoftheseltersastheyrelatetorobotlocalizationfromKalmanltermethodstoadaptivemethods.Eachsectionwilldiscussthecomputationalcomplexity,keyassumptionsthathavetobemadeandtheaccuracyofthelter,aswellasthemoregeneralstrengthsandweaknessesoftheapproach.Examplesofroboticsystemsthatutilizeeachmethodwillbeanalyzed.Fuzzytechniqueswillalsobediscussed-howfuzzylogicandfuzzycontrollershavemadeanimpactontheadaptivemethods,aswellashowSLAMorsimultaneouslocalizationandmapping,whichdominatedthediscussionoflate,usestheselters.TheSLAMproblemisalargerproblemthanjustlocalizationitself,buttheyareverycloselyrelated.2.1Non-AdaptiveFilters2.1.1KalmanFiltersandDerivativesWhytheKalmanFilter?ItistherstchoiceforanylinearsystemwithGaussiannoise,asitisaperfectestimatorundertheseconditions.Itisalsoabletobeexecutediterativelyandinacomputer,withoutextensivelymodifyingthelterorusingspecialhardware.TheKFisthemomentsparameterizationofthe 5

PAGE 18

Bayeslter-usingthemeanandcovarianceoftheGaussianinordertoreducetheequationintoacomputableform.ItisforthesereasonsthattheKalmanFilterandlterslikeithavedominatedforthelast40years.2.1.1.1ExtendedKalmanFiltersTheKalmanFilterhassomeproblems-itrequiresthatitsinputbelinearanditsnoisebeGaussian.Therealworldisrarelylikethisasmostsystemsarenon-linear.TheExtendedKalmanFilterEKFwasdevelopedalmostimmediatelytorelaxoneofthoseassumptions:itlinearizestheinputstothelter,allowingfornon-linearsystemstobemodeled.Atitshighestlevel,theEKF,liketheKF,consistsofTimeUpdatestepandaMeasurementUpdate,oralternatelyPredictandCorrectsegments.IthasacomputationalcomplexityofOk3ingeneral,wherekisthedimensionofthemeasurementvectorzt.ThiscanbebroughtdownaslowasOk2:4withnewmatrixinversionalgorithmsThrun,Burgard,andFox2005.2.1.1.2ExtendedInformationFiltersTheExtendedInformationFilterEIFisthedualoftheEKF-thereexistsabipartitematchingbetweenthemomentsparameterizationusedbytheEKFandthecanonicalparameterizationusinganinformationmatrixandaninformationvector,and,therefore,betweenboththeKFandIFaswellastheEKFandtheEIF.Issues/situationsthataredifculttorepresentinoneltertypearegenerallysimpletorepresentintheother-anexampleofthisisrepresentingglobaluncertainty.IntheEKF,thisamountstoacovarianceofinnitevalue,whileintheEIF,thisisrepresentedbysettingtheinformationmatrixtozero.However,theEIFrequiresmultiplematrixinversionsinboththepredictionandupdatestepwhichgenerallyrequiresmorecomputationalresourcesthanacomparableEKFThrun,Burgard,andFox2005.TheEIFisaconstantmmorecomputationallyexpensivethanastandardEKF-m=3inmostimplementations.Someoftheseproblemscanbeworkedaround,buttheEIFstillremainsmuchlesspopularthantheEKF.2.1.1.3UnscentedKalmanFiltersTheUKForUnscentedKalmanFilterisarelativelynewdevelopmentthatchangesthelinearizationmethodfromarst-orderTaylorSeriesexpansiontothatofasecond-orderTaylorSeriesexpansiontheunscentedtransformorUTJulierandUhlmann1997;JulierandUhlmann2002;Julierand 6

PAGE 19

Uhlmann2004.TheUTcaneasilybeextendedtohigherorderexpansionsaswell.ThegeneralcaseofexpandedTaylorSeriesexpansionsisdescribedbyNgaard,Poulson,andRavnNgaard,Poulsen,andRavn2000.TheUKFisatypeofthemoregeneralsigma-pointKalmanlterSPKFclass.Itselectssigmapointsthatrepresenttheprobabilitydistributionandshareitsmeanandcovariance.TheSPKFrepresentsthelinearstateaswellastheEKFandismuchmoreaccuratethantheEKFinthenon-linearstate.Italsorequiresnoderivativecalculationsorisderivative-free,acrucialrequirementforlow-poweredcomputersystems.ItisalsoaconstantfactormorecomputationallycomplexthantheEKFinmanyimplementations.2.1.2KalmanFiltersforRobotLocalizationKalmanltershavearichhistoryofbeingusedforlocalizationandsensorfusion.Borensteinetal.providesagoodoverviewofearlyresearchdoneinindoorrobotlocalizationusingKalmanltersBorenstein,Everett,andFeng1996.Muchofthisresearchinvolvesaggregatingmultiplesensorintoafusedwhole:multipleultrasonicsensorsCrowley1989,odometryandrangendersCox1991,odometryandcamerasMarinakisetal.2007,odometryandinertialnavigationsystemsINSAshokarajetal.2004,andlaserrangenderwithcamerasArrasetal.2001.Thesearethenintegratedwithanapriorimap.TheresultsreportedbyArrasetal.wereparticularlyimpressive,withover145,000localizationcyclesinacluttered,dynamicenvironmentandzerolostsituations.Overall,theseallfoundEKFtobeaneffectivemethodofsensorfusionasitrelatestoindoorrobotlocalization.Outdoorlocalizationalgorithmsrelyonbothsensorfusionand/ormapmatchingusingEKF.BarshanandDurrant-WhyteperformlocalizationusingEKFfrommultipleINSBarshanandDurrant-Whyte1995andSukkarieh,NebotandDurrant-Whytedescribeanavigation/localizationmethodusingGPS,INSandEKFsensorfusionSukkarieh,Nebot,andDurrant-Whyte1999.BothofthesemethodsrelyoncombiningthevarioussensorsviaEKFwithalargeamountofhandtuning.TheGPS-IMUfusionpaperaddressesmultipatherrors-theerrorcausedbybadsatellitexesduetooccludedsky-astheyrelatetorobotlocalization,solvingtheproblemwithachi-squareddistributiontestappliedovertheinnovationstodetermineifthenewestGPSxshouldbeincludedinthelter.Astochastic-cloningKalmanlterwasrecentlydevelopedbyMourikis,RoumeliotisandBurdick,enhancingabasicEKFwithadditionalinformationextractedbycorrelatingtheproprioceptivesensorswiththeexteroceptiveones,aswellasseparatingtheproprioceptiveupdatesfromtheexteroceptiveonesMourikis,Roumeliotis,andBurdick2007.Thisapproachyieldedimprovementsofover500timesversusodometryalone.AnEKFfusingthewheelencoders,INS,andasunsensorwasdevelopedfortheMarsRoverbyRoumeliotisandBekeywhichimprovedtheaccuracyofthelocalizationbymorethan3.5 7

PAGE 20

timesRoumeliotisandBekey1997.Cappelleetal.fuseINS,GPS,and3D-GISoutdoormapinformationinaninnercityenvironmentwhereGPSreceptioncanbespottyatbest.Theymaintainalocalizationaccuracyofwithin3metersevenwithlongtermGPSoutageswhereINSalonewouldbealmostimmediatelylostCappelleetal.2007.2.1.2.1SLAMandKalmanFilterLocalizationRecently,alargeamountofresearchhasbeenfocusedontheSLAM,orSimultaneousLocalizationandMapping,problem.SLAMisamoredifcultproblemthanjustlocalization-therobothasnomapoftheenvironmentnordoesithaveaxonitspose.Itonlyknowsthecontrolsthatitprovides,oru1:t,andthemeasurementsitreceives,orz1:t.Thereisnowaytoreferencethisinformation,exceptbymakingitselfreferential.However,insomecases,thetotaldifcultyoftheproblemisreducedbyusingaffectionsintheenvironment-knownlandmarksandgloballylocalizedsensors.TheproblemofoutdoorSLAMisinsomecases,simplerthanthatofindoorSLAM-theworld-frame,exteroceptivesensorsofGPSandcompassprovideatleastsomeinformationinordertodeterminetherobot'spose.SomeoftheadvancementsinSLAMcanbedirectlyappliedtolocalizationalgorithms,andanyimprovementinlocalizationwillimproveSLAMaswell,astheyarecloselyrelated.Smith,Self,andCheeseman'sseminalpaperlaidoutthegroundworkforthemodernSLAMproblemusingEKFSmith,Self,andCheeseman1990.Multi-ratesensorfusionforbothUKFandEKFisexploredbyArmestoandTorneroArmestoandTornero2004,whileRodriguez-Losadaetal.examinethefailingsofEKF-SLAMtobuildlargeindoormapsRodriguez-Losadaetal.2007.2.1.2.2SPKFforRobotLocalizationRudolphvanderMerwe'sPh.D.thesis,Sigma-PointKalmanFiltersforProbabilisticInferenceinDynamicState-SpaceModels,isathoroughexplanationofSPKFandtheiraccuracy,derivation,andcomputationalcomplexity.HedemonstratesaSPKFfusionofaGPS,magneticcompass,INS,andaltimeter.Itachievedupto65%betterresultsthanatraditionalEKF.vanderMerwe2004.ApaperbyAshokarajetal.presentsasimulationofarobotequippedwithultrasonicsensors,wheelencodersandanINSinertialsensor.ThissimulationusesIntervalAnalysisIAasamethodofprovidingaglobalupdatetothesensorsystemandshowsthatittsthegroundtruthbetterwithIAthanwithoutAshokarajetal.2004.ApaperbyZhangetal.givesresultsfromusingaUKFtofuseGPS,INS,andadigitalcompass. 8

PAGE 21

However,theycannotcomparetheresultstothegroundtruth,andhavenoharddatatobackuptheirclaim.Zhangetal.2005.TorneroandVinczeshowthatUKFisnotthebestchoiceforallapplications-theirpaperdiscussesthefusionofcameradatawithanINS.BothUKFandEKFprovidesimilarresults,buttheUKFhasacomputationalcostseventimeshigherthantheEKFTorneroandVincze2007.2.2AdaptiveFiltersTherearetwomajorproblemswiththefamilyofKalmanFilters:theyrequirehandtuning-apossiblypainfuliterativeprocess-andtheycannotadapttoachangeinthesystemplantsensorproblemsnorasuddenincreaseinnoiseoruntrustworthinessfromasensor.Severaldifferentmethodsofadaptivelteringhavebeendevelopedtoovercometheseproblems.Theideaofadaptation-thegrowthandalterationofthelter'sstructureorrepresentationofstates-isanimportantstepinthedevelopmentofrobotlocalization.Adaptationcanbebrokendownintotwomajorcategories-inherentadaptationanddrivenadaptation.Inherentadaptationisadaptationthatisinternaltothelterdesign-commonlythettestsurviveviasomekindofevolutionaryprocess.Drivenadaptationisanexternalobserverchangingthelterinternalsonline-alteringthepropertiesofthelteritselfviaacontrolloop.2.2.1InherentAdaptation-ParticleFiltersAnalternativetotheKFanditsderivativesaresequentialMonteCarlomethods,ofwhichtheParticleFilterPFisthemoststudiedandpopular.TheMonteCarlomethodwasrstdescribedbyMetropolisandUlamin1949MetropolisandUlam1949,whiletheCondensationalgorithmbyIsardandBlakeIsardandBlake1998engenderedtherecentspateofparticlelterpapersinlocalizationandSLAM.Particleltershavethefollowingcharacteristics:theyareapproximate,non-parametric,andcanmodelnonlineartransformationsofrandomvariablesThrun,Burgard,andFox2005.Whiletheirapproximatenaturecancauseproblemswhendealingwithaverynarrowclassofproblems,PF'sabilitytomodelnonlineartransformationsofrandomvariablesandbeingnon-parametricmorethanmakeupforitinaverybroadclassofproblems.Theparticlelterrepresentstheposteriorbeliefbyasetofrandomstatesamplesdrawnfromit.Theserandomsamplesarecalledparticlesandeachisapossiblehypothesisoftheworldstateattimet.Themoresamplesthatllasubregion,themorelikelythatthissubregionisthetruestate.InthePFequivalentofthePredictstep,eachparticleisupdatedintoahypotheticalstatefromitscurrentstateandthecontrolvalueut.Thesesamplesarethenweightedbasedonthemeasuredstatebeforehavinga 9

PAGE 22

newsetofparticlesdrawnfromtheprevioussetwithaweightedprobability.ThisistheequivalentoftheMeasurementstepintheKFfamily.TheSequentialImportanceSamplingalgorithmSISDoucet,Gordon,andAndrieu2000isthesequentialMCtechniquethathasformedthebasisformuchoftheparticlelterresearchoverthepastdecade-beingrepurposedorreinventedinmanydifferentguises-CondensationIsardandBlake1998,bootstraplteringGordon,Salmond,andEwing1995,particlelteringorsurvivalofthettestsamplingKanazawa,Koller,andRussell1995.VanderMerweetal.proposeanUnscentedParticlelter,usinganUKFastheproposaldistribution.Thisprovidesa6timesdecreaseinltererrorversusastandardparticlelterinsimulationvanderMerweetal.2000.2.2.1.1ParticleFiltersandRobotLocalizationParticlelterlocalizationwasrstintroducedbySimmonsandKoenigviaGrid-basedMonteCarloLocalizationMCL.Foralongtime,thiswasconsideredthestateoftheartforlocalizationofmobilerobots.Foxetal.hasagoodoverviewofthiseraoflocalizationviaparticleltersFoxetal.1999.AgloballocalizationmethodbasedonthissystemcalledDynamicMarkovLocalizationwasdevelopedbyBurgardetal.in1998Burgardetal.1998.Grid-basedMCLnallyfellbythewayside,spurredbythedevelopmentofthecondensationalgorithm,andFoxetal.developedthersttrueparticleltersforrobotlocalizationFoxetal.1999.Doucetetal.andDoucet,GordonandAndrieudiscussaRao-BlackwellisedParticleFilterorRBPFDoucetetal.2000;Doucet,Gordon,andAndrieu2000.ThislterusesEKForUKFtoculloutlyingparticlesGrisetti,Stachniss,andBurgard2007.PerformingMCLwithwithcamerashasbeenexploredbyLenserandVelosowhoalsoexaminedthesensorresettingtechniqueLenserandVeloso2000,SchulzandFoxSchulzandFox2004,andWolfetal.Wolf,Burgard,andBurkhardt2005.MCLandMHEKFMultiHypothesisEKF,whichwillbediscussedinthenextsectionareexaminedbyGutmannandFoxGutmannandFox2002.TheyfoundthatMCLandMHEKFperformcomparablywell,withMCLmorecomputationallycomplexthanMHEKF,butthemoreadaptiveMCLhandlesnoisebetter.Whenusingparticlelterstoperformrobotlocalization,thereareafewproblems-solvingthekidnappedrobotproblem,perfectsensorscausethesystemtofail,andchoosingthenumberofparticlestouseisoftendifcult.Solvingthekidnappedrobotproblemisanimportantgoalforanyrobotlocalizationscheme.Whilemanyrobotsmaynotbetrulykidnapped,ormovedfromonelocationtoanotherinstantaneouslywithnosensorupdate,solvingthisproblemprovesthatifarobothadaselectionofsensorfailuresthatwerethenrecoveredfrom,therobotcouldresumelocalizing.Thewaytosolvethisproblem 10

PAGE 23

withMCListoinjectrandomparticlesintothesystembasedonlocalizationperformance.Inshort,iftheaveragemeasurementprobabilityisdifferentfromtheprobabilityofsensormeasurementsgivenyourcurrentmeasuredstateandtheinputs,thenmoreparticlesshouldbeadded.Luckily,theaveragemeasurementprobabilityiseasilyapproximatedas1 MPMm=1w[m]tpztjz1:t)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;u1:t;m.Thisneedstobeaveragedoverseveraltimestepsinordertoeliminateoutliers.AmethodcalledAugmentedMCLadaptsboththeshorttermandlongtermaverageofpztjz1:t)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;u1:t;mwithanexponentialdecayfactorGutmannandFox2002.Thissolvesthekidnappedrobotproblemaswhentheaveragemeasurementprobabilityisdifferentfromtheprobabilityofsensormeasurementstherobotisnotwhereitthinksitis.Thealgorithmtheninjectsalargenumberofnewrandomparticlesthatquicklyconvergetothenewstate.Strangely,perfectsensorscauseMCLtofail,evensensorsthatcannotlocalizearobotdirectly.Thisisduetosamplingbias-theperfectsensorreducesthenumberofstatesyoucansamplefromuntilyoucannotsampleintothetruestategiventheinputs.Theeasysolutiontothisistojustaddrandomnoisetoallofthesensorsinthesystem.Considerthisatypeofprocessnoisefortheactualparticlelter.However,thismethodisclunkyandrequirestuninginordertodeterminethebestamountofnoisepersensor.Abettermethodistoswitchwhatisdrivingtheparticlesforasubsetofalloftheparticles-movetheseparticle'sproposaldistributionaccordingtothemeasurementsreceivedx[m]tpztjxtandcalculatetheimportancedensityorweightsfromthemotionmodelw[m]t=R[x[m]tjut;xt)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1dxt)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1.Thisnewmethodisincrediblyinefcientbyitself,butbyincludingitasasmallsubsetabout5%ofthetotalparticles,itpreventssensorbiasfromcrashingthenumberofparticlesintheMCL.ThismethodisknownasMCLwithmixtureproposaldistributionorMixtureMCLThrun,Fox,andBurgard2000.Particlelterswithxednumbersofparticlesareinefcient.Iftheyhavetoofewparticles,theltercannotrecoverfromerroneousdataorbeingkidnapped.Theyneedenoughparticlesspreadovertheentirestatespacetobeabletorecoverfromthiskindoferror.However,thisparticlespreadreducestheaccuracyofthelterandincreasesthecomputationalcomplexity.Therearetwomethodsthatareusedtovarythenumberofparticlesinthesystem.Byvaryingtheparticlenumbers,thecomputationalcomplexityandtheerroroftherepresentationcanbevariedaswell.TherstmethodisLikelyhood-basedAdaptationFoxetal.1999.Likelyhood-basedadaptationLBAisveryintuitiveandfunctionsassuch:ifthevaluesreturnedbythesensorsmatchwellwiththetargetdistributionorthesampleweightsarelarge,thenshrinkthenumberofparticles,astheparticlelterobviouslyrepresentsthetrueposteriorwell.Ifthetargetdistributionandthesensordatadonotmatchwellorthesampleweightsaresmall,thenincreasethenumberofparticles,randomlydistributedaboutthestatespace,asthePFisnotrepresentingtheposterior 11

PAGE 24

well.Thismethodissuperiortoxedsamplesizelters,butithastwoproblems.First,itisconfusedbyevenlydistributedclusters,likesixparticleclusterssettledaroundasinglepoint.Theyallhavesimilarweightsgiventheimportancedensity,butaredenitelynotwellrepresentativeoftheposterior.Second,sensorfailureswreckhavocwiththissystem,asthesensorscontinuallyreportthesamevalueornovalue.Whilethisshouldpushthenumberofparticleshigheratalltimes,itisnotguaranteedtofollowapace.AbettermethodgiventhesedrawbacksisknownasKLDSamplingKullback-LeiblerDistanceSamplingFox2003.ThetricktothismethodistoboundtheerrorbetweenthePFandthetrueposteriorwithacertainprobability.Moreformally,thiscanbestatedasfollows-Foreachiteration,whatisthenumberofsamplesS,sothatwithprobability1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(theerrorbetweenthePFrepresentationandthetrueposteriorislessthan".Inordertoprovethisforageneralcase,assumethatthetrueposteriorisadiscrete,piecewiseconstantdistribution.OnemustdemonstratethatthedistancebetweentheMaximumLikelyhoodEstimateMLEofthesamplesandthetruedistributiondoesn'texceed".TheproofisinapaperbyFoxFox2003.InaparticlelterwithKLD-Sampling,kcanbeestimatedduringsamplingbycountingthenumberofbinswithsupport.Ifbinsareempty,moreparticlesareadded.Thesebinsdivideupsensor/mapspaceneartherobotframe.Intheend,itmeasurestheerrorbetweenthetrueposteriorandthesamplerepresentationPFandusesthattoalterthenumberofparticles.2.2.1.2ParticleFiltersandSLAMParticleltershaveproventobethemostrobustmethodforsolvingtheSLAMproblem.ThecurrentbestofbreedincludeFastSLAM2.0Montemerloetal.2003,whichisanimportantupdatetoFastSLAMasitprovablyconvergesandsolvessomeissueswiththeoriginalMontemerlo2003,Marginal-SLAMMartinez-Cantin,deFreitas,andCastellanos2007,aUPF-UKFunscentedPF,unscentedKFapproachWangandZhang2007,andanoptimalcontrolbasedsolutionLiuetal.2007Recently,theSLAMproblemhasbeenextendedtotheSimultaneousLocalization,MappingandMovingObjectTrackingSLAMMOT.ThisinvolvesperformingtheSLAMproblemwhilesimultaneouslyrunningaPFtotrackmovingobjects,andrequiresseparatingmovingandnon-movingobjectsintheworldWangetal.2007.ApaperbyWangandZhangdemonstratesaUPF-UKFlterforSLAManditoutperformsFastSLAM2.0inasimulationoftheirdesign.Itappearstoprovidebetterresultsinhighlynon-linearenvironmentswithlotsofnon-linearnoiseWangandZhang2007. 12

PAGE 25

2.2.2InherentAdaptation-MultipleHypothesisEKFAtypeofKalmanlteringalsoinvolvesinherentadaptation-MHEKForMultipleHypothesisExtendedKalmanFilterThrun,Burgard,andFox2005.ThisisalsoknownasMultipleModelAdaptiveEstimationMohamedandSchwarz1999.AbankofdifferentKalmanltersdifferentQkandRkareusedwiththesamedata.Thesearemonitoredtoseewhichhasthelowestinnovationovertimeandweightedaccordingly.Thisweightedvalueisthenusedasthenaloutput.ThismethodresultsinafarsuperiorvaluecomparedtoastandardEKF,especiallywithunknownorpoorlytnoisevalues.However,thetradeoffisincomputationalcomplexityandstoragerequirements-itnotonlyrequiresawaytochoosehowtoweightthelters,butitalsorequiresmultiplelters-eachoneaddingtotherequirements.Inmanyways,theMHEKFisanaloguetoaparticlelter,justwithmoreintelligentparticlesandnocullingReuter2000;RoumeliotisandBekey2000;JensfeltandKristensen2001;CoxandLeonard1994.Pushingtheideaofintelligentparticlestotheextremeistheparticleswarm.Eachparticleinthismethodcontainsitsownlogicorcontrolandseekstoreasonabouttheworld-interactingwithotherparticlesandthecontrolinputsaswell.Thismethodhasseensuccessintheeldofglobaloptimizationandsearch,withwiderangingapplicationsKennedyandEberhart1995;Zhang,Yu,andHu2003;MaslovandGertner2006.2.2.2.1MultipleHypothesisEKFandRobotLocalizationRoumeliotisandBekeyproposeasystemthatuniesBayesianestimationandKalmanltering-itutilizesasetofKalmanlterstotracktheproprioceptivesensors-encoders,visualodometryandINSandfusesthemwithBayesianestimationfrommap-basedfeatureextractionviaexteroceptivesensors.Whileitsupposedlyperformstestsofthisalgorithmusingarealrobot,thereisnoinformationabouttheirrobotnordotheyshowanypicturesoftheirexperimentalsetupRoumeliotisandBekey2000.ReuterproposesapureEKFsolution,usingmultipleEKFsandproposedhypothesisthatarethenweightedbasedontheirmeasurementdistance.Heshowshisresultsinbothsimulationandexperiments-within20stepsitconvergestounambiguouschoiceReuter2000.JensfeltandKristensenalsoimplementaMHEKFandhavelesssuccessoverallthanReuter-requiringmanymoretimestepsandevenfailingtoconvergeseveraltimes.However,theydoprovideausefulcomparisonbetweenMHEKForMHLversusParticleFilterMCLJensfeltandKristensen2001: 13

PAGE 26

MHLprovidesamoreintuitiverepresentationwithasmallnumberofposehypotheses.ItisnotaseasytoemployactiveexplorationstrategiesforMCLasitisforMHL.MCLhastheadvantageofbeingabletoincorporateallkindsofdata,whereasMHLrequiresthataGaussianapproximationispossible.MHLisbetterattakingadvantageofstrongfeatures.IntheinitialphaseMCLmustexplicitlymodeltheglobaluncertaintybydistributingsamplesthroughouttheenvironmenttherebyspendingmassiveamountofcomputations,whereasthezero-hypothesisofMHLtakescareofthiseffortlessly.2.2.3DrivenAdaptation-Innovation-BasedAdaptiveEstimationTheothertypeofadaptationisdrivenadaptation,oradaptationofthelterinternalsdrivenbyanexternalobserver.Asearlyas1969thereweremultipleproposalsforcontrollingthedivergenceinKalmanFiltersbychoosingthesystemandmeasurementnoisecovariancematriciesQkandRkappropriately.ThisisknownasInnovation-BasedAdaptiveEstimationorIAEMohamedandSchwarz1999.ThesemethodsfocusonreducingtheresidualsorerrorbetweenthemeasuredstateandtheestimatedoneJazwinski1969;Mehra1970.2.2.3.1Innovation-BasedAdaptiveEstimationandSensorFusionRobertsetal.proposeaneurofuzzysystemforestimatingQkandRkbeforethelterissettorunRobertsetal.1994.AndersonsuggestsexponentialdataweightingtopreventolddatafromdominatingtheKF-newerdataisgivenmoreprecedencethanolddata.ThiscorrespondstoanassumptionofincreasedinputnoiseAnderson1973.MehraproposesInnovation-BasedAdaptiveEstimation-alteringthelterastheinnovationssequence,orltermeasurementresiduals,change.AslidingestimationwindowtakestheresultsfromthelterandaltersQkandRkappropriatelyMehra1970;Maybeck1979.MohamedandSchwarzproposeaMLorMaximumLikelyhoodestimatorinordertoadaptgiventheinnovationsequence.Thisisgoodfortheirparticularapplication,buttherestrictivenatureoftherequirementsmakeitinapplicableformobilerobotlocalization,especiallywithbadsensorsorerroneousdataMohamedandSchwarz1999. 14

PAGE 27

2.2.3.2Innovation-BasedAdaptiveEstimationandRobotLocalizationAbdelnourproposesafuzzylogicsupervisorFLStomonitortheinnovationssequence.TheFLSdetectsiftheinnovationsarenolongerpurelyrandomoroutsideoftheacceptablepocketandperformscorrectiveaction,alteringQkandRktoreducethedivergenceAbdelnouretal.1993.Thiskindofonlinelteringisfurtherdevelopedbyasuccessionofresearchers-Kobayashietal.Kobayashietal.1998,SasiadekandWangSasiadekandWang1999,HsuHsu2000,Linetal.Linetal.2003,PrattandKramerPrattandKramer2007andLinandTsaiLinandTsai2007.Hsu,Lin,andLinandTsaiallusetheExtendedInformationFilterEIF,whichisthedualoftheEKF-reportingsimilarresultsamarkedimprovementoverstandardEIFforindoorlocalization.SasiadekandWangdemonstrateasimulatedFuzzyEKFFEKFsystemusingINSandGPSsensors.Kobayashietal.demonstratesaFEKFsystemusingadifferentialGPSDGPS,ayaw-rategyroscopeandaspeedsensor,butdonotprovidequantitativeresults.PrattandKramerprovideaFEKFsystemanddemonstrateusingrealdataanimprovementinaccuracyofupto5.57timesoverthestandardEKFwithaboutatwotimesincreaseinruntime,whichisthebestmeasureofcomputationalcomplexitygiventheunknownimplementationofthefuzzylogicenginePrattandKramer2007.PrattandKramerusedtheinitialvaluesofQandR,theerrorcovariancematrix,andthemeanoftheresidualdifferencebetweentheprediction^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(kandthemeasurementzkforagiventimesteptogenerateaweightingconstanttomodifyQandR.Thisadaptabilityallowsittoescapethekidnappedrobotsituation-thelargeincreaseintheinnovationforcesthesystemtobemoreopentoincomingmeasurementsveryquickly,andonceithasstabilized,allowsittoreturntosmallercovariances.Reinaetal.useafuzzylogicsystemtoprovidesomeintelligencetoafadingmemorymethodofminimizingdivergenceReinaetal.2007.TheirsensorshapingFEKFltersaDGPSandachievesa75%improvementoverastandardEKF.ThismethodowesquiteabittoAnderson'sexponentialdataweighting.Shenetal.demonstrateaDoubleFuzzyKalmanFilterDFKF,whereQkandRkeachhaveseparatefuzzycontrollers.Theyshowanimprovementinaccuracyofover3timesversusastandardEKFwithrealdatafromatractorShen,Zhu,andMao2007.2.2.3.3DrivenParticleFilterEvenparticleltersystemscanhavedrivenadaptation.Kwok,FoxandMeilaproposeadrivenAdapativeRealTimeParticleFilterRTPFthatchangesthenumberofparticlesandcalculationsbasedonthecomputationalresourcesavailableandtheaccuracyofthelterKwok,Fox,andMeila2004.Thislets 15

PAGE 28

themachievea77%increaseinaccuracyoverastandardparticlelteratextremelylowprocessingpower.FuzzyadaptiveparticleltershavebeenrecentlyexploredbyAsadian,Moshiri,andSedighAsadian,Moshiri,andSedigh2005andKimetal.Kimetal.2007.Asadian,Moshiri,andSedighuseafuzzycontrollertoalterNkorthenumberofparticlesontheyviamonitoringtheresiduals.Iftheresidualsofthelterareoutsideoftheenvelope,thecontrollereitheraddsorsubtractsparticles.ThisismuchlikeafuzzyIAEapproachandwasonlytestedinsimulationAsadian,Moshiri,andSedigh2005.Kimetal.proposeasimiliarsystem,butuseindoorbeaconsandvelocityfromanINS.Theyshowasmallincreaseinaccuracy,butdonotdiscussifthereisanytradeoffinvolvedincreasedcomputationalcomplexityKimetal.2007.Duan,CaiandYuproposeanevolutionaryadaptiveparticlelter,wheretheparticlesareweightedviatheresiduals,andthenareevolvedrandomlymuchlikeinageneticalgorithmtomaintainparticlediversity.Thisresultsinanalmost4timeincreaseinaccuracyofthelterinsimulationversusastandardparticlelter.TheevolutionaryparticlemodicationcanprovidealmostdoubletheaccuracyinsomecasesDuan,Cai,andYu2007.2.2.4FuzzyAdaptiveFiltersOfthesetwomethodsofdrivenadaptabilityfuzzyandtraditionalcontrol,fuzzymethodshavebeenthesubjectofmorerecentandintenseresearch.Thisisduetotwomainreasons-easeofimplementationandrobustness.Fuzzylogicsystemsareeasytoimplementandareinherentlyhumanreadable.Astandardsystem,likeaPIDcontroller,isdifcultforhumanstounderstandandtherequiredpartsmaybeill-dened.Afuzzymethodloosenstherequirementsfortotalsystemclassication.Fuzzysystemsarealsomorerobust,especiallyinthecaseabove-theMLestimatorinMohamedandSchwarz1999isextremelyfragile.Also,thestandardsystemismuchmoresensitivetooutlyingdata.Asensorglitchcandrivetheinnovationtoahighvalue,forcingalargeswinginQkandRk.ThesefuzzycontrolmethodsarenotlimitedtoKalmanlters-fuzzifyingparticleltersandhavingfuzzycontrollersofanypartoftheparticlelterareanimportantnewavenueofresearchAsadian,Moshiri,andSedigh2005.2.3ConclusionsSeveralmajortypesofltershavebeendiscussedsofar:Kalmanltersandvariants,inherentlyadaptiveltersanddrivenadaptivelters.Eachoftheseltershavetradeoffsandbenetsinregardstotheiruseforrobotlocalization.EKFsrequireGaussianerror,haveaninaccuratelineralizationprocessandare 16

PAGE 29

non-adaptive.However,theyhavelowcomputationalcomplexity,Ok2:4,areperfectestimatorsforlinearsystems,andareeasytoimplement.EIFshavemanyofthesamedrawbacksastheEKF,andareaconstantfactormorecomputationallyexpensive.Theyarealsomoredifcultforhumanstounderstandandimplement.SPKFshaveafarmoreaccuratelinearizationprocess,freeingthemfromtheGaussianrequirement,aswellasbeingderivativefree,animportantrequirementforsmallchipsystemsthatmaylackadedicatedderivativesolver.TheyarealsoaconstantfactormorecomputationallyexpensivethananEKF.InherentadaptiveltersincludeparticleltersandMHEKF.Particleltersareareapproximate,non-parametric,andcanmodelnonlineartransformationsofrandomvariables.Theyarealsoeasytoimplementintheirbaseform.However,withouttheextracomplexityofMixtureMCLandKLDSampling,theyarepronetoerrorsoreventotalmalfunction.Theycanalsobeincrediblywastefulofcomputationcycles-manyparticlesmaybenecessaryinordertogetagoodestimation.MHEKFcanbebetterthanPFinsomesituations,buthasbeenlargelysubsumedbyPFresearch,mostlyduetotheinuenceofSLAM.Insmallerposehypothesissituations,theMHEKFissuperior,butasthenumberofhypothesesincrease,theMHEKFgainscomputationalcomplexityfasterthanthePF-Omk2:4,wheremisthenumberofhypotheses.Drivenadaptation,andespeciallyfuzzydrivenadaptation,ismoreexpensivethanastandardEKF,butbyaconstantfactor.Thegainsrealizedbyadrivenadaptivelterareimpressive,uptomorethan5timesasaccuratewhencomparedtoastandardlterofitstype.Giventhecurrentstateoftheart,therearenocomparisonstudiesandverylittleintrospectioninmostpapersinregardstowhythisparticularlterisbetterthanothersandwhatnicheitshouldbeusedin.Thisdeciencyisaseriousissueimpactingthedevelopmentoffuturerobotsystems.Theintegrationoffuzzylogiccontrollersandtherefore,ltersthathavedrivenadaptationisrapidlygrowingandforgoodreason:fuzzylogiccontrollersencapsulateandsimplifywhatmightbeanimpossiblydifcultsystemcharacterization.Theyprovideamethodtoreduceinformationinacontrolledway,givingnecontrolovertheinformationtothrowawayandincreasingthecompatibilitybetweendisparatesourcesofinformation.Localizationisoneofthemostimportantproblemsarobotneedstosolve.Unlessarobotknowswhereitis,itcannotdoanyusefulwork.Therehasbeenalargeamountofinterestinsmallroboticsystemswithlimitedpower,computationalresourcesandpayloads.Thiswillnecessitatesmaller,lessaccuratesensorsandlowerpoweredprocessors.Inorderlocalizetheserobots,researcherswillneedtodeveloppowerful,robust,andyetlightweightalgorithms. 17

PAGE 30

Inthenextchapterwecomparedifferentlteringtechniquesandalgorithms,givingaquickoverviewoftheassumptions,drawbacksandbenetsofeachltermethod. 18

PAGE 31

Chapter3ComparativeAnalysisofLocalizationAlgorithms 3.1IntroductionThereareseveralkeylimitationsinherentinprovidingaccuratelocalizationforasmallrobot-limitedspace,limitedprocessingpower,andlimitedpower.Asmallrobotdemandssmallsensors,bothforpowerandspacelimitations.Asasensorgrowssmaller,itgenerallygrowsmoreinaccurate-oneofthemostaccuratesensorsystems,theTrimbleDSM232,hasafootprintof19.6centimetershigh,by14.8centimeterswide,by21.6centimetersdeep.Itisaccuratewithin10centimeters.Incomparison,thetypeofGPSthatmaybeusedonapalm-sizedrobotistheu-bloxUBX-G5010,withafootprintofonly8millimetersby0.9millimetersby8millimeters.However,itonlyhasanaccuracyofabout2.5meters.Also,thelimitationofprocessingpowerdemandsasensorfusionmethodthathasalowcomputationalcostwhileremainingbothaccurateandrobust.Itisinthislocusofrequirementsthatonemustconsidertheirchoicesforsensorfusionfromamongsttheavailablelters.Asshownintheprevioussection,thereareahugenumberofBayesian-familyltersandvariations-eachoftheseisapossiblesolutiontotheproblemofaccuratelocalizationgivenuncertainsensors.Howdoesonechoosebetweenthem?Thischapterwilldiscussthetradeoffsandassumptionsinherentineachltertype,aswellasthecostsandbenetsofpossibleextensions.Thepossibilityofabestlterforaspecicsituationwillalsobediscussed;howcanonebecertaintheyhavefoundtheproperltertype?Whiletheideaofthebestlterisanappealingone,researchersarebetterservedbydiscussingthemostappropriatelter-onethattstheassumptionstheymakeandhascoststhatarenottooonerous.Whilethisparticularprobleminvolveschoosingthemostappropriatelterforasmallrobot,thisthesiscanserveasaguidelinetofutureresearcherslookingtochooseanappropriatelterfortheirparticularapplication,nomatterwhatitmaybe. 19

PAGE 32

3.2KalmanFiltersandDerivativesKalmanltersofalltypestintotheunadaptablelterfamily-thoselterswhosecoredesigndoesnotalteroverthecourseoftheapplication.Whileunadaptablehasanegativeconnotation,unadaptablealsomeansfastandsmall.Thelackofacontrollermakeshandtuningimportant,butforcertainapplications,thisisbyfarthebestchoice.ThebasicKalmanlteristhemostsimpleandthefastestofthelterspresentedhere,butithasseveredrawbacks-allofthenoisemustbeGaussianandtheinputsandoutputsmustbelinear,asshowninTable4.However,undertheselimitations,itisaperfectestimator-aclaimnoneoftheotherlterscanmake.Itisalsoabletobeexecutediterativelyandinacomputer,withoutextensivelymodifyingthelterorusingspecialhardware.TheKFisthemomentsparameterizationoftheBayeslter-usingthemeanandcovarianceoftheGaussianinordertoreducetheequationintoacomputableform. Table4.KalmanFilterAnalysis Assumptions Drawbacks Benets Thesystemisentirelylinear. Allinputsandoutputsarelinear. Thisisthefastestlter,ontheorderofOk2. TheerrorinthesystemmustbeGaussian. ThelessGaussiantheerroris,thelesslikelythelterwillcon-verge. Givenassumptions,KFisaper-fectestimator. 3.2.1EKFIftherealworldwasprimarilylinear,theKFwouldbethebestchoiceinmostsituations.However,theworldisalmostneverlinear,andtheExtendedKalmanFilterEKFwasquicklydevelopedtorelaxthisconstraint.WhilethecomputationalcomplexityincreasestoOk3,duetothematrixinversion,thiscanbereducedtoOk2:4-closetotheOk2complexityofthestandardKFThrun,Burgard,andFox2005.3.2.2EIFItiseasiertorepresentcertainconceptsintheExtendedInformationFilterEIF,suchasglobaluncertainty-intheEKF,thisamountstoacovarianceofinnitevalue,whileintheEIF,thisisrepresentedbysettingtheinformationmatrixtozero.TheEIFisthedualoftheEKF-thereexistsabipartitematchingbetweenthemomentsparameterizationusedbytheEKFandthecanonicalparameterizationusingan 20

PAGE 33

Table5.ExtendedKalmanFilterAnalysis Assumptions Drawbacks Benets Theinputiscontinuous. Mustlinearlizetheinput. Extremelyfastlter. TheerrorisGaussian. ThelessGaussiantheerroris,thelesslikelythelterwillcon-verge. Withlinearinputs,theEKFmatchestheKF-itisaperfectestimator. Requiresamatrixinversionandderivatives. TheEKFisthebaselinetowhichallotherltersarecompared. informationmatrixandaninformationvector,andthereforebetweenboththeKFandIFaswellastheEKFandtheEIF.However,theEIFrequiresmultiplematrixinversionsinboththepredictionandupdatestep-generallyrequiringmorecomputationalresourcesthanacomparableEKFThrun,Burgard,andFox2005.ItisaconstantmmorecomputationallyexpensivethanastandardEKF-m=3inmostimplementations. Table6.ExtendedInformationFilterAnalysis Assumptions Drawbacks Benets Theinputiscontinuous. Hardtoimplementanduse. CaneasilysolvesomeproblemsthatareextremelydifcultforEKF. TheerrorisGaussian. Makescertainproblemsmoredifcult. Constantfactormorecomputa-tionallycomplexthanstandardEKF. Requiresmultiplematrixinver-sionsandderivatives. AsaccurateasEKF. 3.2.3SPKFSigmaPointKalmanFilterSPKFarethenewesttypeofKalman-derivedltersandhaveeliminatedmanyoftheissuesthatplagueothertypesofKalmanlters.Ithingesontheselectionofsigmapointsorasetofpointsfromaprobabilitydistributionthatperfectlycapturesthecovarianceandmeanofthedensityequation.TheSPKFrepresentslinearstatesaswellastheEKF,andthenon-linearstatesmuchbetterthantheEKF.ItdoesnotrequirethenoisetobeGaussian.Italsorequiresnoderivativecalculations 21

PAGE 34

orisderivative-free,acrucialrequirementforlow-poweredcomputersystems.ItisalsoaconstantfactormorecomputationallycomplexthantheEKFinmanyimplementations-Ok3,wherekisthedimensionofthemeasurementvectorzt.Withtheapplicationofthesquare-rootformulation,thiscanbefurtherreducedtoOk2. Table7.SigmaPointKalmanFilterAnalysis Assumptions Drawbacks Benets Inputiscontinuous. Moredifculttounderstandandimplement. Derivativefree. Requiresamatrixinversionandsquarerootcalculation. Requiresmultiplesquare-rootcalculationswithstandardde-sign. Modelsnon-linearstatesmuchbetterthanEKF. CanbeclosetoOk2withthesquare-rootformulation. 3.3AdaptableFiltersTherearetwomajorproblemswiththefamilyofKalmanFilters:theyrequirehandtuning-apossiblypainfuliterativeprocess-andtheycannotadapttoachangeinthesystemplantsensorproblemsnorasuddenincreaseinnoiseoruntrustworthinessfromasensor.Severaldifferentmethodsofadaptivelteringhavebeendevelopedtoovercometheseproblems.Theideaofadaptation-thegrowthandalterationofthelter'sstructureorrepresentationofstates-isanimportantstepinthedevelopmentofrobotlocalization.Adaptationcanbebrokendownintotwomajorcategories-inherentadaptationanddrivenadaptation.Inherentadaptationisadaptationthatisinternaltothelterdesign-commonlythettestsurviveviasomekindofevolutionaryprocess.Drivenadaptationisanexternalobserverchangingthelterinternalsonline-alteringthepropertiesofthelteritselfviaacontrolloop.Thesesystemsalwayspresentacomplexity/accuracytradeoff-theyaregenerallymoreaccurate,atthecostofincreasedcomplexity.Thecomplexitytoaccuracyratioisrelativelylinearfortheinherentsystems-themoreevolutionaryitemsyouprovide,themoreaccuratetheyare.Theratioismoreobscurefordrivensystems-dependingonthesituation,thecontrollercouldbeeithercrucialoruseless.Incertaincases,thecontrollercanevenbe 22

PAGE 35

counterproductive-ifthemodelofthesystemiswrongortheinputsareambiguous,thecontrollercandrivethesystemintoanunusablestate.3.3.1InherentAdaptation-ParticleFiltersAnalternativetotheKFanditsderivativesaresequentialMonteCarlomethods,ofwhichtheParticleFilterPFisthemoststudiedandpopular.TheanalysisofparticleltersareshowninTable8.Particleltersareapproximate,non-parametric,andcanmodelnonlineartransformationsofrandomvariablesThrun,Burgard,andFox2005.Whiletheirapproximatenaturecancauseproblemswhendealingwithaverynarrowclassofproblems,theparticlelter'sabilitytomodelnonlineartransformationsofrandomvariablesandbeingnon-parametricmorethanmakeupforitinaverybroadclassofproblems.Thereareproblemswithsamplingbias,choosingtherightnumberofparticles,andhowtoderivetheprobabilitydensityfromtheparticlecollection-eachoftheseissolvablewiththeproperextensiontothetraditionalformulaMixtureMCL,KLDSampling,DensityExtractionMethods,buteachinvokesacostforthesolution.Usuallythiscostisincomputationalcomplexity,butsometimesitalsoincreasesinaccuracy,especiallyinthecaseofMixtureMCL. Table8.ParticleFilterAnalysis Assumptions Drawbacks Benets Answerisanapproximation. Basicdesignispronetoavarietyofproblems. Canmodelnon-lineartransfor-mations. Canhaveahugecomputationalcost,especiallyatstartup. Isnon-parametric. Allsolutionstoissueswithbasicdesignhaveacomputational/ac-curacycost. Easytoimplement. 3.3.2InherentAdaptation-MultipleHypothesisEKF/SPKFAnotherinherentlteringmethodinvolvesmultipleEKFsorSPKFs-MHEKForMultipleHypothesisExtendedKalmanFilter,theanalysisofwhichisinTable9.AsetofdifferentEKForSPKFareprovidedthesamedata.Theinnovationofeachismonitoredandtheyareweightedinorderoflowestinnovation.Theweightedvalueisthenusedasthenaloutput.Thismethodresultsinafarsuperiorvalue 23

PAGE 36

comparedtoastandardEKF,especiallywithunknownorpoorlytnoisevalues.However,thetradeoffisincomputationalcomplexityandstoragerequirements-itnotonlyrequiresawaytochoosehowtoweightthelters,butitalsorequiresmultiplelters-eachoneaddingtotherequirements.Inmanyways,theMHEKFisanaloguetoaparticlelter,justwithmoreintelligentparticlesandnoculling. Table9.MultipleHypothesisExtended/SigmaPointKalmanFilterAnalysis Assumptions Drawbacks Benets Answerisanapproximation. Canhaveahugecomputationalcost,Omk2:4wheremisthenumberofhypothesisgeneratinglters. NegligiblestartupcostversusPF. ErrorisGaussian. Easytodevelopactiveexplo-rationstrategies. 3.3.3DrivenAdaptation-Innovation-BasedAdaptiveEstimationTheothertypeofadaptationisdrivenadaptation,oradaptationofthelterinternalsdrivenbyanexternalobserver.TheanalysisofthislterclassisinTables10and11.Asearlyas1969,thereweremultipleproposalsforcontrollingthedivergenceinKalmanFiltersbychoosingthesystemandmeasurementnoisecovariancematricesQkandRkappropriately.ThesemethodsofcontrollingdivergenceareknownasInnovation-basedAdaptiveEstimationorIAE.Thesemethodsfocusonreducingtheresidualsorerrorbetweenthemeasuredstateandtheestimatedone.Thetwomajormethodsaretraditionalcontrolsandfuzzycontrols.Thetraditionalcontrolmethodworksonaslidingestimationwindowinordertoeliminateoutliers,whilethefuzzycontrolmethodfavorsamoderatecontrolupdatemethod-slowlyiteratingQkandRkupordown.3.4ConclusionsThissectionservesasaguideforfutureroboticsresearcherswhoarelookingforthemostappropriatelocalizationalgorithm,however,wemustconsiderourparticularproblemarea-small,palm-topsizedrobotswithuncertainsensors.WhilewetestallofthesealgorithmsinforsuitabilityinChapter5,wecanchooseourbestoptionsfromjustthisguidealone.WecanfeasiblydiscountboththeParticleFiltersandMultipleHypothesisoptions,giventheirtraditionallyonerouscomputationrequirements.TheEIFis 24

PAGE 37

Table10.TraditionalInnovation-BasedAdaptiveEstimationAnalysis Assumptions Drawbacks Benets ErrorisGaussian. Requirestwoextramatrixin-versionsandanothersummationversusstandardEKF. Cancontrolresiduals,creatingamuchbetterfusion. Inputiscontinuous. Sensitivetooutliers. Keepslterfromdiverging. Requiresmatrixinversionsandmaybederivatives. Canprovideaworseanswerver-sushand-tuning. Table11.FuzzyInnovation-BasedAdaptiveEstimationAnalysis Assumptions Drawbacks Benets ErrorisGaussian. Requiresafuzzylogiccon-troller. Cancontrolresiduals,creatingamuchbetterfusion. Inputiscontinous. Unknowncomputationalcostforthefuzzycontroller. MuchlesssensitivetooutliersthanstandardIAE. Requiresmatrixinversionsandmaybederivatives. Canprovideaworseanswerver-sushandtuning. RequireslessknowledgeofthesystemtoprovideagoodcontrolsolutionforQkandRk. 25

PAGE 38

removedfromconsiderationasitprovidesmuchthesameresultsastheEKF,yetismorecomputationallycomplex.BothadaptableandbasicEKFandSPKFappeartobemostappropriatetoourproblem-fast,lightweightalgorithms.ThiswillplayoutinChapter5-theSPKF,FEKF,andMHFEKFareourbestchoicesforalgorithms. 26

PAGE 39

Chapter4SensorFilterImplementation 4.1IntroductionInordertodeterminethemostappropriatelterforaccuratelocalizationgiventhespecicrequirementsofsmallrobots,onemusttestdifferentlters.Thischapterwilldiscusstheimplementationofvariousltersinordertoproduceafuseddataset-thealgorithmsandhowtheltersarestructuredinordertoprovidethefusedoutput.Thesystemanalyzedisoverconstrainedbyfourseparatesetsofsensors-anIMU,acompass,onboardodometryandaGPS.Thisoverconstraintprovidesanopportunitytocascadethelters-derivinganevenbetterresultbylteringeachstage.ThismodelisshowninFigure1.4.2KalmanFiltersandDerivativesAlloftheKFderivativesrelyonthePredictandMeasurementsteps-therststepcalculatesthepredictedstategiventheinput,andthesecondcalculatesthefusedstategiventhemeasurement.Whenperformingsensorfusion,theinputtotherststateisgenerallyarateofchangeinthemeasurement-avectorcorrespondingtothevelocityorrotationalvelocity.Thesecondstateisusuallyanabsolutemeasurementtowhichtheupdatedstateiscompared.ThenexttwosubsectionswillshowthealgorithmsfortheEKFandtheSPKF.Bothofthesearecascadedtogether,likeinFigure1-multipleEKForSPKFareneededforthecompletefusiononeforeachsensor.TheEIFisnotdiscussedasitsparticularpropertiesarenotofbenetinthissituationanditsdrawbacksarelargeenoughtomakeitnotacontender.4.2.1ExtendedKalmanFilterTostarttheprocess,injectinitialvaluesfor^xk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1,aprioripositionestimate,andPk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1,apriorierrorcovarianceintotheTimeUpdateequations. 27

PAGE 40

Figure1.EKF/SPKFSensorFusionMethod 28

PAGE 41

4.2.2EKFEquationsIntheEKF,xkandzkaretheactualstateandmeasurementvectorsofthefusedsensors.~xkand~zkaretheapproximatestateandmeasurementvectorsforthefusedsensorresult,showninEquationsand.ukisthedrivingfunctionandwkandvkrepresenttheprocessandmeasurementnoise,respectively.Thesenoisevaluesareassumedtohavenormalprobabilitydistributions,beindependentofeachother,andbewhite.BeforeonecanbeginusingtheEKF,oneneedstodeneeachpartoftheoverallalgorithm.Theapproximatestateandmeasurementvectorsofthefusedsensorsaredenedas:~xk=f^xk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;uk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;0~zk=h~xk;0ThevariousJacobiansthatarerequiredinordertoalterthesystemormeasurementstoshareadataspaceareinEquationsand,whiletheJacobiansthatarenecessaryinordertoattributethesystemandmeasurementerrortotheoverallsystemareinEquationsand:A[i;j]=@f[i] @x[j]^xk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;uk;0W[i;j]=@f[i] @w[j]^xk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;uk;0H[i;j]=@h[i] @x[j]~xk;0V[i;j]=@h[i] @v[j]~xk;0Oncethesystemhasbeeninitializedwiththesevalues,thesystembeginsthecycleofPredictandMeasurementupdates.Thesystembeginsbyprojectingthestateahead:^x-k=f^xk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;uk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;0Thesystemthenprojectstheerrorcovariance,Pk,aheadaswell:P-k=AkPk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1ATk+WkQk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1WTk 29

PAGE 42

ThenthesystemproceedstocorrecttheestimatesmadewithaMeasurementUpdate1.First,theKalmangain,Kkissolved:Kk=P-kHTkHkP-kHTk+VkRkVTk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1TheestimateisthenupdatedwiththemeasurezkinEquation,followedbytheerrorcovarianceupdateinEquation,beforereturningtothetoptoPredictupdatesagain:^xk=^x-k+Kkzk)]TJ/F11 9.9626 Tf 9.9626 0 Td[(h^x-k;0Pk=I)]TJ/F32 9.9626 Tf 9.9626 0 Td[(KkHkP-k4.2.2.1DivergenceWhilethelinearilizationoftheinputsoftheKFgreatlyimprovestheperformanceofthelterinnon-linearworlds,itintroducesanotherproblemintheformofdivergence.Divergenceoccurswhentheactualerrorinthesystemislargerthantheerrorcovariancematrixrepresents.Thisforcesnewmeasurementdatatobegivenlessandlessweightor,inthisapplication,makesthelterrelymoreheavilyondirectsensorinputversusthecalculatedinterimpositionsandcancausearapiddecreaseinsystemaccuracy.Abdelnouretal.identifythreesourcesofdivergence-truedivergencecausedbyunboundedsystemmodelingerrors,apparentdivergencecausedbyvariationsinthedatainput,systembiasandsomesystemmodelingerrors,andnumericaldivergencecausedbyroundingerrorsAbdelnouretal.1993.Whilenumericaldivergencecanbehandledviaavarietyofmathematicalmethods,trueandapparentdivergencearemuchmoredifculttohandle.Truedivergencearisesfromtheoriginalsystemmodelbeingerroneous.Thishappensifthesensorsaremischaracterizedor,morelikely,therobot'smotionmodeliswrong.Also,theoptimalityofthelterisstronglyconnectedtothequalityoftheaprioriinformationabouttheupdatemeasurementandprocessnoise.Erroneousaprioriinformationprovidedtotheltercausesdivergenceandbiasinthelter.IfQk,thesystemnoisecovariance,orRk,themeasurementnoisecovariance,aretoosmall,thenthesystemwillformaGaussianestimationthatistoothin,resultinginabiasedandincorrectanswer.Iftheyaretoolarge,thelterdiverges,perhapsuncontrollably.Also,thewrongchoicesofnoisestatisticscaneitherpromoteordestroyweaklyobservablecomponents,evenwhentheywillresultinsuperioranswersorcauseuncontrollableerrorsMohamedandSchwarz1999.Intheend,divergenceand 1ThePredictionandMeasurementupdatesdonotnecessarilyneedtorunina1:1cycle,thecontrolinputPredictionandmeasure-mentupdatesmaybemadeatarbitrary,non-correspondingrates. 30

PAGE 43

biasareextremelydifcultproblemstosolve,yetadaptivemethodsmayhelpamelioratethem-thosewillbediscussedbelow.4.2.3SPKFTheconstructionoftheSPKFisremarkablysimilartotheEKF-bothconsistofPredictionandMeasurementsteps,buttheSPKFtakestheextrastepofcalculatingasetofsigmapoints.Theinitialmeanandcovariance:^x0=E[x0]Px0=E[x0)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^x0x0)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^x0T]areaugmentedinto:^xa0=E[xa]=E^x000TPa0=E[xa0)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^xa0xa0)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^xa0T]=266664Px0000Rv000Rn377775TheformfollowstheEKFafterthat,withbothPredictionandMeasurementStep.Choosingaproper,,andisimportant.mustbe0orgreatertoensurethatthecovariancematrixremainspositive-=0isacceptable.shouldbesmallandwithin01.Asmallminimizespossiblehigherordereffects.isusedtobringknowledgeofhigherordermomentsintothedistribution.Thiscontrolstheheavinessofthetailsintheposteriordistributionaswell.NotethatListhedimensionofx,Rvistheprocessnoisecovariance,Rnisthemeasurementnoisecovarianceandisthecompositescalingparameter.AsummaryofthevariablesinvolvedisinTable12. Table12.SPKFVariables Variable Description Minimizeshigherordereffectswithinthesystem Controlsforhowlongtailsareintheposteriordistribution Mustbe0orgreatertoensureapositivecovariancematrix x Thesystemvariables,inthiscase,ourlocation Thesigmapoints Thecompositescalingparameter L Thedimensionofx Prescaledweightingparameter 31

PAGE 44

ThealgorithmishardertounderstandthantheEKF,butcreatessuperiorresults.First,onedenesxaas:xa=xTvTnTTandaas:a=xTvTnTTandarebothderivedvalues-calculatedbeforehandtoeasenotationandforreusablility:=2L+)]TJ/F11 9.9626 Tf 9.9626 0 Td[(L=p L+Eachsigmapointisweightedbeabletoextracttherelativecontributiontotheoverallfunction,asthesetofsigmapointsexactlyqualiestheoriginalfunction'smeanandcovariance.Theseweightsarecalculatedbelow,withwmiastheweightsforthemeans,whilewciaretheweightsforthecovariances:wm0= L+wc0= L++)]TJ/F11 9.9626 Tf 9.9626 0 Td[(2+wmi=wci=1 2L+i=1;:::;2LNowthatwehavedenedallnecessarypreconditions,thealgorithmisreadytobeused.First,calculatethesigmapoints;thesquareofthedegreesoffreedomplusoneistheminimumnumberofsigmapointsrequired.TheibelowdesignatestheroworcolumnofthematrixPathatisbeingaddressedforthatcalculation:ak)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1=^xak)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1^xak)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1+p Pak)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1^xak)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(p Pak)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1iThealgorithmthenproceedsmuchliketheEKFwouldhave,withaPredictionupdate:xkjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1=fxk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;vk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;uk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(k=2LXi=0wmixi;kjk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1 32

PAGE 45

P)]TJ/F33 6.9738 Tf 0 -6.5764 Td[(xk=2LXi=0wcixi;kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(kxi;kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(kTThispredictionstepisthenfollowedbyaMeasurementupdate:'kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1=hxkjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;nk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1^y)]TJ/F10 6.9738 Tf -0.1591 -7.2671 Td[(k=2LXi=0wmi'xi;kjk)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1P~yk=2LXi=0wci'xi;kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1)]TJ/F8 9.9626 Tf 10.5748 0.1384 Td[(^y)]TJ/F10 6.9738 Tf -0.1592 -7.2671 Td[(k'xi;kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1)]TJ/F8 9.9626 Tf 10.5748 0.1384 Td[(^y)]TJ/F10 6.9738 Tf -0.1592 -7.2671 Td[(kTPxkyk=2LXi=0wcixi;kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1)]TJ/F8 9.9626 Tf 10.4953 0.1384 Td[(^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(k'xi;kjk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1)]TJ/F8 9.9626 Tf 10.5748 0.1384 Td[(^y)]TJ/F10 6.9738 Tf -0.1591 -7.2671 Td[(kTKk=PxkykP)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1~yk^xk=^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(k+Kkyk)]TJ/F8 9.9626 Tf 10.5749 0.1384 Td[(^y)]TJ/F10 6.9738 Tf -0.1592 -7.2671 Td[(kPxk=P)]TJ/F33 6.9738 Tf 0 -6.5764 Td[(xk)]TJ/F32 9.9626 Tf 9.9626 0 Td[(KkP~ykKTkNotehowtheextractionofthetruefunctionoccursinmultiplepartsoftheSPKF.Anytimeweightsareused,truevaluesandcovariancesarebeingextracted-seeEquations,,,,and.Also,pleasenotethatthearrayinEquationmustbesquareanddiagonal-placethevaluesinorderalongthediagonal,asshownbelowinEquationfora3degreeoffreedomSPKF:Pa0=26666666666666666666666664Px000000000Py000000000Pz000000000Rvx000000000Rvy000000000Rvz000000000Rnx000000000Rny000000000Rnz37777777777777777777777775 33

PAGE 46

4.3ParticleFiltersTheSequentialImportanceSamplingalgorithmSISDoucet,Gordon,andAndrieu2000isthesequentialMCtechniquethathasformedthebasisformuchoftheparticlelterresearchoverthepastdecade-beingrepurposedorreinventedinmanydifferentguises-CondensationIsardandBlake1998,bootstraplteringGordon,Salmond,andEwing1995,particlelteringorsurvivalofthettestsamplingKanazawa,Koller,andRussell1995.ThemethodusedwillbebasedontheSISalgorithmwithalloftheenhancementsnecessaryinordertoremoveerrors.Abasicdiagramofthelter'sdesignisFigure2.Inthisalgorithm,fxi0:k;i=0;;Nsgisasetofpointswithweightsfwik;i=1;;Nsg.Thesetofallstatesuptotimekisx0:k=fxj;j=0;;kgandtheposteriorpdfpx0:kjz1:kischaracterizedbyarandommeasurefxi0:k;wikgNsi=1.Theweightsareallnormalized,sothatPiwik=1.Thisallowstheposteriordensityatktobeapproximatedas:px0:kjz1:kNsXi=1wikx0:k)]TJ/F11 9.9626 Tf 9.9626 0 Td[(xi0:kThisisadiscreteweightedapproximationtotheactualposterior,px0:kjz1:kandtheweightsarechosenbyimportancesamplingDoucet1998.Giventhatpx/xisdifculttodrawsamplesfrom,butcanbeeasilyevaluated,letqbeanimportancedensityfromwhichonecandrawsamplesequaltoxiqx;i=1;;Ns.Theweightedapproximationtothedensitypis:pxNsXi=1wix)]TJ/F11 9.9626 Tf 9.9626 0 Td[(xiwhere:wi/xi qxiisthenormalizedweightattheithparticle.Ifoneusestheimportancedensityqx0:kjx1:ktodrawthesamplesxi0:k,thendenestheweightsintobe:wik/pxi0:kjz1:k qxi0:kjz1:kInthesequentialcase,thesamplesfromtheprevioustimestepconstituteanapproximationofpx0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1.Fromthis,onewantstoapproximatepx0:kjz1:kforanewsetofsamples,updating 34

PAGE 47

themforthistimestep.Ifonefactorizestheimportancedensity:qx0:kjz1:k=qxkjx0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;z1:kqx0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1thenthesamplesxi0:kqx0:kjz1:kareobtainedbyusingthenewstatexikqxkjx0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;z1:ktoaugmenteachexistingsamplexi0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1qx0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1.Theweightupdateequation,pxi0:kjz1:k,isderivedbyrstbeingexpressedintermsofpx0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1,pxkjxk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1,andpzkjxk:pxi0:kjz1:k=pzkjx0:kjz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1px0:kjz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1 pzkjz1:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1=pzkjx0:kjz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1pxkjx0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1 pzkjz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1px0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1=pzkjxkpxkxk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1 pzkjz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1px0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1/pzkjxkpxkxk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1px0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1 Whenandaresubstitutedinto,theweightupdateequationbecomes:wik/pzkjxikpxikjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1pxi0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1 qxikjxi0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;z1:kqxi0:k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1jz1:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1=wik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1pzkjxikpxikjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1 qxikjxi0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;z1:k Ifqxkjx0:k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;z1:k=qxkjxk)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zk,itmeansthatthehistoryofallpreviousstatesiscompletelycontainedinthecurrentstate.ThisisknownastheMarkovassumption-theideathatalloftherelevantstatedataisrolledupbythepreviousestimations.Thisassumptionisheldacrossalloftheselters.Thisisveryusefulinthisparticularlterasitallowsustoreceivealteredestimateofpxkjz1:k,anditalsoallowsustodiscardthehistoryofobservationsandthepathfollowed,maintainingonlythecurrentstate.Themodiedweightisthen:wik/wik)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1pzkjxikpxikjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1 qxikjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zkwhichallowsustoapproximatetheposteriorltereddensitypxkjz1:kas:pxkjz1:kNsXi=1wikxk)]TJ/F11 9.9626 Tf 9.9626 0 Td[(xik 35

PAGE 48

wherewikisgivenbyEquation.Equationapproachesthetrueposteriordensity,pxkjz1:kasNs!1.4.3.1DensityExtractionDensityextractionisnecessaryforanyPF,asitdoesnotreturnanewestimationateachtimesteplikeallKF-basedltersdo.Therearemultipledifferentwaystoperformdensityextraction:simpleparticlemeancalculation,Gaussianapproximationofunimodaldensities,k-meansclusteringformultimodaldensities-approximatingadensitywithamixtureofGaussians,binningbyoverlayingahistogramandsummingweights,andkerneldensityestimationwhichisperformedbyusingeachparticleasthecenterofakernelandthencalculatingthedensityfromthemixtureofthekerneldensity.Thesekerneldensitiescanbechosentobeanytypeofdensity-aGaussianforeachkernelisapopularchoiceThrun,Burgard,andFox2005.Allofthesemethodsofdensityextractionhavecomputationalcomplexityandaccuracytradeoffs.4.3.2ParticleDeprivationandDegeneracyParticledeprivationisaseriousproblemforallparticlelters.Thelossofgeneralityintheparticleset,reducingdowntoonlyafeworasingleparticle,aswellasthedegeneratecaseofallbutasingleparticlewithnegligibleweightistroublingbecauseitmeansthattheltercannolongeradapttochangingconditionsPittandShephard1997;Gordon,Salmond,andEwing1995;Rubin1987;Berzuinietal.1997.Theparticleltercanenterastatewheretherearenoparticlesnearthecorrectstate.Thebruteforcemethodofxingtheparticledeprivationproblemistosimplyaddmorerandomparticles.Thisisimpracticalandcausesunacceptablenoiseinsomecases.Therearetwomajorwaystodealwiththedegeneracyproblem-choosingagoodimportancedensityandresampling.Theimportancedensityallowstheweightsofthevariousparticlestobeeasilydetermined-itprovidestheupdatesteptotheparticlelterinasingleequationandcutsdownonthecomputationalcomplexity.However,determiningwhattheimportancedensityshouldbeisacrucialstep,andadifcultone.MinimizingVarwik,orthevarianceoftheparticleweights,inordertomaximizetheeffectivesamplesizeNeffisoneeffectivemethod.DegeneracyisindicatedbyasmallNeff,andNeffisalwayslessthanNs,ortheactualnumberofsamplesLiuandChen1998.Itisdenedas:Neff=Ns 1+Varwik 36

PAGE 49

Figure2.ParticleFilterSensorFusionMethod where:wik=pxikjz1:k qxikjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zkWhilecannotbecalculateddirectly,anestimateNeffisavailablewithwikthenormalizedweightfrom:Neff=1 PNsi=1wik2AsshowninDoucet1998,theoptimalimportancedensityfunctionistheonethatminimizesthevarianceofthetrueweightswikgivenxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1andzk:qxkxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zkopt=pxkxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;xk=pzkjxkxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1pxkxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1 pzkxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1 37

PAGE 50

Substitutingintoobtains:wik/wik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1pzkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1=wik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1Zpzkjxkpxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1dxk Nomatterwhatsampleisdrawnfromqxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zkoptforagivenxik)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1,wikisalwaysthesamevalue,makingthisanoptimalchoicefortheimportancedensity.However,thisisdifculttoimplementinthegeneralcase,asitrequirestheabilitytoevaluatetheintegralinoverthenewstate,aswellastheabilitytosamplefrompxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zk.Therearetwospeciccasesinwhichitmaybepossibletousethisoptimalimportancedensity-forJump-MarkovlinearsystemsoranytimexkisamemberofanitesetDoucet,Gordon,andKrishnamurthy2001bandwhenpxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zkisGaussianDoucet1998.Whileitmaynotbepossibletousetheoptimalimportancedensityinmanysituations,itispossibletodesignsuboptimalapproximations.ThisfollowsthesamemodeloftheKF-locallinearizationtechniques.OnemethodusesaGaussianapproximationofpxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zk,muchlikeanEKFDoucet1998.AnotherusesanestimateoftheGaussianapproximation,usingtheunscentedtransform,likeanUKFvanderMerweetal.2000.Anotherpopularmethodofchoosingtheimportancedensityistomakeittheprior:qxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;zk=pxkjxik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1Whenonesubstitutesintoityields:wik/wik)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1pzkjxikThechoiceoftheimportancedensityisofvitalimportanceinordertoachieveahighqualityresultfromtheparticlelter.Whiletheseapproximatemethodsworkwellattimes,atunedimportancedensityforeachparticularapplicationwillbalancetheparticledensitywiththeper-particlecomputationalcostsandprovidethebestresultsArulampalametal.2002.Apoorchoiceofanimportancedensityequationwilldestroyaparticlelter,astheparticledensityinareasofinterestwillbereducedtozero,ineffect,freezingtheltertoasingleparticlewhichmayormaynotbethetruestate. 38

PAGE 51

4.3.3ResamplingandSamplingVarianceResamplingistheprocessofadaptingthecurrentbeliefstate,orparticledistribution,totthenewmeasuredresults,whichhavealreadybeenincorporatedintotheimportancedensity.Theparticlelterusestheimportancedensityasaweightedprobabilityfunctionwhenitdrawsanequalnumberofparticlestoreplacethecurrentset.Thishastheeffectoftransformingtheolddistribution,ortheproposaldistribution,intothenewdistribution,orthetargetdistribution.Thegoalisthetargetdistribution,butitisonlypossibletodrawsamplesfromtheproposaldistribution.Theimportancedensityisusedasaweightingfactortopushtheparticledistributiontowardsthetargetdistribution.However,theactofresampling,especiallywhennotneeded,cancauseparticledeprivation.Resamplingcausesparticledeprivationduetosamplingvariance,orthevariationinrandomsampling.Thisvariationmakesanitegroupofsamplesdrawnfromaprobabilitydensityhaveaslightlydifferentsetofstatisticsthananothercopyofthosesamples.Overtime,theydivergemore,evenunderstaticconditions.Eventually,particleseitherdivergeentirelyandrepresentanuntrueprobabilitydensityfunctionortheparticlesystemcollapsestoasingleparticle.Thisvarianceisalsoampliedbyrepetitiveorinappropriatesampling-drivingthesystemtodivergeorcollapsesooner,especiallyunderano-inputsituation.Thelargeryourgroupofsamples,themoreresistanttovarianceyourparticlesystembecomes.However,thisincreasesthecomputationalcomplexityandruntimeofthelter.LesscostlysolutionsinvolvereducingthenumberofresamplesandusingalowvariancesamplerasequentialstochasticprocessArulampalametal.2002;Thrun,Burgard,andFox2005.SeethecodeinAppendixAunderPF.javaforimplementationdetails.Reducingthenumberofresamplescanbemoreofanartthanascience,asthegainsfromreducingresamplingcanvarywildlydependingonapplicationorenvironmentalsituation.Onekeyistonotresamplewhilethestateisstatic.Inmanyroboticapplications,thisisimportantandrelativelyeasy,butisimpossibleinothers.Itisalsopossibletoreduceresamplingbymultiplicativelyintegratingmeasurementsintotheimportanceweightsinsteadofresamplingaftereachupdate.Thisisalsomostapplicableforprimarilystaticsystems.Lowvariancesamplingisapowerfulmethodofreducingsamplingvariance-providinggainsinbothaccuracyandcomputationalcomplexity.InsteadofselectingMrandomparticlesbyaskingarandomnumbergeneratorforMvalues,onecaninsteadaskthissamegeneratorforasinglevalue,r,andtheniteratethroughtherestoftheparticlespaceinorder.Thishasseveralbenets:theparticlespaceissystematicallycovered,reducingvariance,iftheimportancedensityfortheproposaldistributionisthesame 39

PAGE 52

asthetargetdistribution,nochangesaremadeandnosamplesarelost,anditperformstheresamplingstepinOMtimeinsteadofOMlogM.Anothersimilarmethodthatperformsbetterwithmultiplehypothesesisthestratiedsampling.Inthismethod,samplesaregroupedintosubsets.Thesesubsetsarethengrantedacertainnumberofparticlestodistributegiventhetotalweightofeachsubset.Theseparticlesarethendistributedviathelowvariancemethodwithineachsubset.WRSorWeightedRandomSamplingisanotherexcellentlow-variancesamplercreatedbyEfraimidisandSpirakis-theirA-ExpJalgorithmonlyrequiresOmlogn=mrandomvariablesinsteadofOnEfraimidisandSpiraki2008.4.3.4SamplingBiasSamplingbiasisanotherproblemwithlownumbersofparticles.TheactofsamplingitselfreducesthedimensionalityoftheposteriordensityproposaldistributionfromthenumberofparticlesMtoM-1,astheweightsofthelastparticleintargetdistributioncanbederivedfromtheweightsofalltheotherparticles.AtlowvaluesofM,thisisdisastrous,forcingthetargetdistributiontowardspxtju1:tinsteadofpxtju1:t;z1:t,orinotherwords,neglectingthemeasurementinthecalculationofthetargetdistribution.4.3.5KidnappedRobotProblemSolvingthekidnappedrobotproblemisanimportantgoalforanyrobotlocalizationscheme.Whilemanyrobotsmaynotbetrulykidnapped,ormovedfromonelocationtoanotherinstantaneouslywithnosensorupdate,solvingthisproblemprovesthatifarobothadaselectionofsensorfailuresthatwerethenrecoveredfrom,therobotcouldresumelocalizing.ThewaytosolvethisproblemwithMCListoinjectrandomparticlesintothesystembasedonlocalizationperformance.Inshort,iftheaveragemeasurementprobabilityisdifferentfromtheprobabilityofsensormeasurementsgivenyourcurrentmeasuredstateandtheinputs,thenmoreparticlesshouldbeadded.Luckily,theaveragemeasurementprobabilityiseasilyapproximatedas1 MPMm=1w[m]tpztjz1:t)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;u1:t;m.Thisneedstobeaveragedoverseveraltimestepsinordertoeliminateoutliers.AmethodcalledAugmentedMCLadaptstoboththeshorttermandlongtermaverageofpztjz1:t)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;u1:t;mwithanexponentialdecayfactorGutmannandFox2002.Thissolvesthekidnappedrobotproblemaswhentheaveragemeasurementprobabilityisdifferentfromtheprobabilityofsensormeasurementstherobotisnotwhereitthinksitis.Thealgorithmtheninjectsalargenumberofnewrandomparticlesthatquicklyconvergetothenewstate.Strangely,perfectsensorscauseMCLtofail,evensensorsthatcannotlocalizearobotdirectly.Thisisduetosamplingbias-theperfectsensorreducesthenumberofstatesyoucansamplefromuntilyoucannotsampleintothetruestategiven 40

PAGE 53

theinputs.Theeasysolutiontothisistojustaddrandomnoisetoallofthesensorsinthesystem.Considerthisatypeofprocessnoisefortheactualparticlelter.However,thismethodisclunkyandrequirestuninginordertodeterminethebestamountofnoisepersensor.Abettermethodistoswitchwhatisdrivingtheparticlesforasubsetofalloftheparticles-movetheseparticle'sproposaldistributionaccordingtothemeasurementsreceivedx[m]tpztjxtandcalculatetheimportancedensityorweightsfromthemotionmodelw[m]t=R[x[m]tjut;xt)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1dxt)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1.Thisnewmethodisincrediblyinefcientbyitself,butbyincludingitasasmallsubsetabout5%ofthetotalparticles,itpreventssensorbiasfromtampingtheMCL.ThismethodisknownasMCLwithmixtureproposaldistributionorMixtureMCLThrun,Fox,andBurgard2000.4.3.6ChoosingtheRightNumberofParticlesParticlelterswithxednumbersofparticlesareinefcient.Iftheyhavetoofewparticles,theltercannotrecoverfromerroneousdataorbeingkidnapped.Theyneedenoughparticlesspreadovertheentirestatespacetobeabletorecoverfromthiskindoferror.However,thisparticlespreadreducestheaccuracyofthelterandincreasesthecomputationalcomplexity.Therearetwomethodsthatareusedtovarythenumberofparticlesinthesystem.Byvaryingtheparticlenumbers,thecomputationalcomplexityandtheerroroftherepresentationcanbevariedaswell.TherstmethodisLikelyhood-basedAdaptationFoxetal.1999.Likelyhood-BasedAdaptationLBAisveryintuitiveandfunctionsassuch:ifthevaluesreturnedbythesensorsmatchwellwiththetargetdistributionorthesampleweightsarelarge,thenshrinkthenumberofparticles,astheparticlelterobviouslyrepresentsthetrueposteriorwell.Ifthetargetdistributionandthesensordatadonotmatchwellorthesampleweightsaresmall,thenincreasethenumberofparticles,randomlydistributedaboutthestatespace,asthePFisnotrepresentingtheposteriorwell.Thismethodissuperiortoxedsamplesizelters,butithastwoproblems.First,itisconfusedbyevenlydistributedclusters,likesixparticleclusterssettledaroundasinglepoint.Theyallhavesimilarweightsgiventheimportancedensity,butaredenitelynotwellrepresentativeoftheposterior.Second,sensorfailureswreckhavocwiththissystem,asthesensorscontinuallyreportthesamevalueornovalue.Whilethisshouldpushthenumberofparticleshigheratalltimes,itisnotguaranteedtofollowapace.AbettermethodgiventhesedrawbacksisknownasKLDSamplingKullback-LeiblerDistanceSamplingFox2003.ThetricktothismethodistoboundtheerrorbetweenthePFandthetrueposteriorwithacertainprobability.Moreformally,thiscanbestatedasfollows-Foreachiteration,whatisthenumberofsamplesS,sothatwithprobability1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(theerrorbetweenthePFrepresentationandthetrueposterioris 41

PAGE 54

lessthan".Inordertoprovethisforageneralcase,assumethatthetrueposteriorisadiscrete,piecewiseconstantdistribution.OneneedstoshowthedistancebetweentheMaximumLikelyhoodEstimateMLEofthesamplesandthetruedistributiondoesn'texceed".ThisproofisoriginallyfromFoxFox2003.ThedistancebetweenthemismeasuredwiththeKullback-LeiblerdistanceKL-distance-astandardmeasureofthedifferencebetweenprobabiltydensitiesordistributions-assuch:Kp;q=Xxpxlogpx qxGivennsamplesdrawnfromtheparticlelterrepresentationoftheposteriorwithkbinsandvectorX=X1;:::;Xkasthenumberofsamplesdrawnfromeachbin,makeXdistributedwithamultinomialdistribution,wherethetrueprobabilityofeachbinisrepresentedbyp=p1:::pk.TheMLEofpwithnsamplesis^p=n)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1Xwhichmakesn,orthelikelihoodratiostatisticfortestingp:logn=kXj=1Xjlog^pj pjAsn^pj=Xj:logn=nkXj=1^pjlog^pj pjGivenand,nisntimestheKLDbetweenthetrueposteriorandtheMLE:logn=nK^p;pThenconvergestoachi-squaredistributionwithk)]TJ/F8 9.9626 Tf 9.9626 0 Td[(1degreesoffreedom:2logn!d2k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1asn!1 TheprobabilityoftheKLDbetweentheMLEandthetrueposteriorbeinglessthanorequalto"withtheassumptionthatpisthetruedistributionisdenotedasPpK^p;p"andisderivedasfollows:PpK^p;p"=PpnK^p;p2n"=Pplogn2n":=P2k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(12n" 42

PAGE 55

andareusedtocalculate.Thequantilesofare:P2k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(12k)]TJ/F7 6.9738 Tf 6.2266 0 Td[(1;1)]TJ/F10 6.9738 Tf 6.2267 0 Td[(=1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(andcanbecombinedaslongasnischosensothat2n"isequalto2k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;1)]TJ/F10 6.9738 Tf 6.2266 0 Td[(sothat:PpK^p;p":=1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(Insummary,ifn,thenumberofsamples,ischosenas:n=1 2"2k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;1)]TJ/F10 6.9738 Tf 6.2266 0 Td[(thentheKLDbetweentheMLEandtruedistributionislessthan"withguaranteedprobability1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(.Inordertousethenthechi-squaredistribution'squantilesmustbecalculated.ThiscanbeapproximatedwiththeWilson-Hilfertytransformation,usingz1)]TJ/F10 6.9738 Tf 6.2267 0 Td[(astheupper1)]TJ/F11 9.9626 Tf 9.9626 0 Td[(quartileofthestandardnormaldistribution,yielding:n=1 2"2k)]TJ/F7 6.9738 Tf 6.2267 0 Td[(1;1)]TJ/F10 6.9738 Tf 6.2266 0 Td[(:=k)]TJ/F8 9.9626 Tf 9.9626 0 Td[(1 2"1)]TJ/F8 9.9626 Tf 26.3615 6.7398 Td[(2 9k)]TJ/F8 9.9626 Tf 9.9626 0 Td[(1+s 2 9k)]TJ/F8 9.9626 Tf 9.9626 0 Td[(1z1)]TJ/F10 6.9738 Tf 6.2266 0 Td[(3InaparticlelterwithKLD-Sampling,kcanbeestimatedduringsamplingbycountingthenumberofbinswithsupport.Ifbinsareempty,moreparticlesareadded.Thesebinsdivideupsensor/mapspaceneartherobotframe.Intheend,itmeasurestheerrorbetweenthetrueposteriorandthesamplerepresentationPFandusesthattoalterthenumberofparticles.4.3.7MultipleHypothesisEKF/SPKFThisisasimplemanipulation/extensionoftheparticleltersystem-replacethefullsetofparticleswithalimitednumberofEKForSPKFlters,eachwithitsownsetofprocessandmeasurementnoisecovariances.TheMHEKForMHSPKFisdenedasSninEquation,whilethesetofresidualsforthelterisdenedasRninEquation:Sn=EKF0;SPKF1;:::;EKFn;SPKFn 43

PAGE 56

Rn=h^x-k;0)]TJ/F32 9.9626 Tf 9.9626 0 Td[(zkEKF0;h^x-k;0)]TJ/F32 9.9626 Tf 9.9626 0 Td[(ykSPKF1;:::Theprocessissimple.First,ndthetotalresidualforthatentry:Rt=FXi=0RiThen,calculatetheweightforeachlter,basedontheratiooftheresiduals:wn=1)]TJ/F11 9.9626 Tf 11.1581 6.7398 Td[(Rn RtFinally,usethatweightinordertocalculatetheweightedaverageofthelters-theweightedlocationvalueinthiscase:^xt=1 wtnXi=0wi^xiThisprocessisrepeatedforeverytimestepthewholesystemgoesthrough.Eachoftheselterscanbeadaptiveornonadaptive.Itactsmuchlikeaparticleltersystemdoes,withmoreintelligentparticlesandlessculling.Thishassomeinterestingadvantagesanddisadvantages,asdescribedbyJensfeltandKristensenJensfeltandKristensen2001: MHLprovidesamoreintuitiverepresentationwithasmallnumberofposehypotheses.ItisnotaseasytoemployactiveexplorationstrategiesforMCLasitisforMHL.MCLhastheadvantageofbeingabletoincorporateallkindsofdata,whereasMHLrequiresthataGaussianapproximationispossible.MHLisbetterattakingadvantageofstrongfeatures.IntheinitialphaseMCLmustexplicitlymodeltheglobaluncertaintybydistributingsamplesthroughouttheenvironmenttherebyspendingmassiveamountofcomputations,whereasthezero-hypothesisofMHLtakescareofthiseffortlessly.4.4DrivenAdaptationTherearetwomajormethodsofprovidingdrivenadaptation,oradaptationfromanexternalcontroller-atraditionalcontrolmethodorafuzzycontroller.BothofthesefallundertheguiseofInnovation-BasedAdaptiveEstimationorIAE.EarlyeffortsbyMehraestablishthebasic,traditionalmethodalteringthelterastheinnovationssequence,orltermeasurementresiduals,change.Aslidingestimationwindow,asshowninEquation,takestheresultsfromthelterandaltersQkandRk 44

PAGE 57

appropriatelyMehra1970;Maybeck1979:^Qk=Kk^CvkKTk^Rk=^Cvk)]TJ/F11 9.9626 Tf 9.9626 0 Td[(HkP)]TJ/F10 6.9738 Tf -1.3837 -7.2671 Td[(kHTk^Cvk=1 NkXj=j0vjvTjTheinnovationorvkiscalculatedasthedifferencebetweentherealmeasurementzkandtheestimatedvalueh^x-k;0,asshownin.Anothermethod,rstproposedbyKobayashiin1998,butfutherrenedbyAbdelnourproposesafuzzylogicsupervisorFLStomonitortheinnovationssequence.TheFLSdetectsiftheinnovationsarenolongerpurelyrandomoroutsideoftheacceptablepocketandperformscorrectiveaction,alteringQkandRktoreducethedivergenceAbdelnouretal.1993.SasiadekandWangdemonstrateasimulatedFuzzyEKFFEKFsystemusingINSandGPSsensors.TheirmethoddidnotadaptQkandRkasabove,butinsteadusedacovarianceweightingfactor,:Rk=R)]TJ/F7 6.9738 Tf 6.2267 0 Td[(2k+1Qk=Q)]TJ/F7 6.9738 Tf 6.2266 0 Td[(2k+1AnothermethodbyPrattandKramerisperformedbyaddingafourthsteptotheEKFequationsabove-PrattandKramer2007:Qk=FLACQbase;Pk;)]TJ/F10 6.9738 Tf 11.2201 4.3835 Td[(kXi=1zi)]TJ/F11 9.9626 Tf 9.9626 0 Td[(x-i=k!Rk=FLACRbase;Pk;)]TJ/F10 6.9738 Tf 11.2201 4.3835 Td[(kXi=1zi)]TJ/F11 9.9626 Tf 9.9626 0 Td[(x-i=k!TheFLACFuzzyLogicAdaptiveControllerisimplementedinthisstep.InEquationsandPrattandKramerusedtheinitialvaluesofQandR,theerrorcovariancematrix,andthemeanoftheresidualdifferencebetweentheprediction^x)]TJ/F10 6.9738 Tf 0 -7.2671 Td[(kandthemeasurementzkforagiventimesteptogenerateaweightingconstanttomodifyQandR.ThefuzzyvaluesusedfortheFLACareshowninFigures3,4,5,and6below.Thisadaptabilityallowsittoescapethekidnappedrobotsituation-thelargeincreaseinthe 45

PAGE 58

innovationforcesthesystemtobemoreopentoincomingmeasurementsveryquickly,andonceithasstabilized,allowsittoreturntosmallercovariances. Figure3.CovarianceMembershipFuzzyInput Figure4.MeanMembershipFuzzyInput TheDFKFDoubleFuzzyKalmanFilterwasdescribedin2007byShen,Zhu,andMao.Itconsistsoftwofuzzylogiccontrollers-onecontrolsRkandtheothercontrolsQk.TheinstantaneouserrorbetweentheIMUandtheuniedsensorresponsefromtherestofthesystemdeterminesthevalueofRk,whilethevalueofQkiscalculatedfromacombinationoftheinnovationofthelateraldeviationandtheinnovationoftheheadingangleasshowninEquations73and74Shen,Zhu,andMao2007.^Zy=g^Zyg+v^Zyv g+v^Z'=v^Z'v+imu^Z'imu v+imu 46

PAGE 59

Figure5.AlphaFuzzyOutput Figure6.FuzzySurfaceoftheFLAC 47

PAGE 60

Betweenthesetwomethodsofdrivenadaptabilityfuzzyandtraditionalcontrol,fuzzymethodshavebeenthesubjectofmorerecentandintenseresearch.Thisisduetotwomainreasons-easeofimplementationandrobustness.Fuzzylogicsystemsareeasytoimplementandareinherentlyhumanreadable.Astandardsystem,likeaPIDcontroller,isdifcultforhumanstounderstandandtherequiredpartsmaybeill-dened.Afuzzymethodloosenstherequirementsfortotalsystemclassication.Fuzzysystemsarealsomorerobust,especiallyinthecaseabove-theMLestimatorinMohamedandSchwarz1999isextremelyfragile.Also,thestandardsystemismuchmoresensitivetooutlyingdata.Asensorglitchcandrivetheinnovationtoahighvalue,forcingalargeswinginQkandRk.ThesefuzzycontrolmethodsarenotlimitedtoKalmanlters-fuzzifyingparticleltersandhavingfuzzycontrollersofanypartoftheparticlelterareanimportantnewavenueofresearchAsadian,Moshiri,andSedigh2005.Adiscussionoffuzzycontrolsystemsandtheoptimalityoffuzzysystemsfollowsinthenextsection.4.5FuzzyControl4.5.1IntroductionAfuzzycontrollerutilizesfuzzysetsandfuzzylogictocontrolaprocess.Fuzzycontrollersareexcellentlowcomplexity,highspeedcontrollersandareespeciallysuitedtotheproblemofprovidingdrivenadaptationtoanonlinelter.SincebeingintroducedbyZadehin1965,fuzzysetsandfuzzylogichaveexplodedintomathematics,controltheory,andmanyotherelds.Atthemostbasiclevel,fuzzysetsaresetsthathaveimpreciseboundaries.Theideaofbelongingtoasetisnolongerabinaryevaluation,butamatterofdegree.Thisnotonlywentagainstprobabilitytheory,butalsotwo-valuedlogic-theideaofbeingabletograduallytransferfrommembershiptonon-membershipandbackisextremelypowerful.Italsoallowsforeasyintegrationofhumanlogicintovarioussystems-conceptssuchasmostly,partially,oralittlecanbeintegrated.SomeexcellentexamplesoffuzzycontrolapplicationsincludeanunmannedlandingsystemandaninstabilityavoidancemodulewithinacontrolloopLivchitzetal.1998;Sanjuan,Kandel,andSmith2006.Whiletechniquessuchasinterval-valuedfuzzysetsandfuzzysetsoftype2havegreatexpressivepowerandutility,theyarealsoextremelycomputationallycomplex,andthereforenotthebestchoicegiventhesegoals.Thisthesiswillinsteadexploretraditionalordinaryfuzzysetsandsystems,theiroptimality,decisionmaking,andcontrol. 48

PAGE 61

4.5.2FuzzySystemsandUncertaintyFuzzyvariableshavevaluesthatarenotxedcrispnumbers,butarerepresentedbyafuzzysetwithintheuniverseofdiscourse,orallofthepossibledatapoints.Thesefuzzysetsdonothavecrispboundaries,insteadtheyrepresentasetofnumbersortheideaofuncertainty.Uncertainty,whilenormallyattributedtoinformation,asinuncertaininformation,isactuallyaconditionoftheagentortheentityperceivingtheinformation.Uncertaintyactuallymeansthattheinformationprovidedtotheagentisnotsufcienttofullycategorizethetruestate.Thereareseveraldifferentweaknessestoinformationthatmaycauseuncertainty-imprecision,vagueness,orunreliability.Impreciseinformationdoesnotprovideenoughinformationtocreateauniquestate.Vagueinformationdoesnotgenerateacrispstate.UnreliableinformationidentiesastatethatmayormaynotexistSafotti1997.Forceduncertaintyoccursduetomeasurementerrorsandthelackofsensorresolution.Whilemeasurementerrorscausinguncertaintyhavebeencoveredindepthsofar,sensorresolutionisasubtle,butimportantissue.Ifasensorisaccuratetoonedecimalplace,itquantizesthesensorandintroducesuncertainty.Acrisprepresentationofthevariablewitharange[0,1]isquantizedintojust11values:0;0:1;0:2;:::;0:9;1:0.Crispsetsdonotrepresenttheuncertaintyassociatedwiththisquantization-itissimplybuiltintotheerroroftheeventualsystem.Fuzzysetscanmoreaccuratelyrepresentthisforceduncertaintyviasmallfuzzysetsencompassingeachmeasuredvalue-[0;0:05;[0:05;0:15;[0:15;0:25;:::;[0:85;0:95;[0:95;1].Thiscanberepresentedlinguisticallybysayingeachvalueisabout0.0,about0.1,about0.2,...,about0.9,orabout1.0KlirandYuan1995.Whileforceduncertaintyisaninevitableoccurrenceduetoinformationdeciency,opteduncertaintymostcertainlyisnot.Opteduncertaintyisachoicetooverquantizeavariabletoreduceittoadesiredlevelofaccuracy.Thisisdoneforthreereasons-tosummarizeinformation,toreducetodecision-relevantinformation,andtolowersystemcomplexity.Bysummarizinginformation,compatibilityisincreasedbetweendisparatetypesofinformation.Reducingasetofvariablestojusttheirdecision-relevantinformationhelpstolimittheinuenceoferroneousdataoroutliers,andloweringsystemcomplexityhelpsspeedcalculations,aswellasmakingcertainproblemstractablegivencomputingresourcesKlirandYuan1995RamerandKreinovich1994.LoftiZadeh,oneoftheluminariesintheeld,providesdeepinsightintowhyfuzzysystemshaveanadvantagegiventhisuncertainworldZadeh1973. Giventhedeeplyentrenchedtraditionofscienticthinkingwhichequatestheunderstandingofaphenomenonwiththeabilitytoanalyzeitinquantitativeterms,oneiscertaintostrikea 49

PAGE 62

dissonantnotebyquestioningthegrowingtendencytoanalyzethebehaviorofhumanisticsystemsasiftheyweremechanisticsystemsgovernedbydifference,differential,orintegralequations....Essentiallyourcontentionisthattheconventionalquantitativetechniquesofsystemanalysisareintrinsicallyunsuitedfordealingwithhumanisticsystemsor,forthatmatter,anysystemwhosecomplexityiscomparabletothatofhumanisticsystems.Thebasisforthiscontentionrestsonwhatmightbecalledtheprincipleofincompatibility.Statedinformally,theessenceofthisprincipleisthatasthecomplexityofasystemincreases,ourabilitytomakepreciseandyetsignicantstatementsaboutitsbehaviordiminishesuntilathresholdisreachedbeyondwhichprecisionandsignicanceorrelevancebecomealmostmutuallyexclusivecharacteristics.Itisinthissensethatprecisequantitativeanalysesofthebehaviorofhumanisticsystemsarenotlikelytohavemuchrelevancetothereal-worldsocietal,political,economic,andothertypesofproblemswhichinvolvehumansasindividualsoringroups.Analternativeapproach...isbasedonthepremisethatthekeyelementsinhumanthinkingarenotnumbers,butlabelsoffuzzysets,thatis,classesofobjectsinwhichthetransitionfrommembershiptonon-membershipisgradualratherthanabrupt.Indeed,thepervasivenessoffuzzinessinhumanthoughtprocessessuggeststhatmuchofthelogicbehindhumanreasoningisnotthetraditionaltwo-valuedorevenmultivaluedlogic,butalogicwithfuzzytruths,fuzzyconnectives,andfuzzyrulesofinference.Inourview,itisthisfuzzy,andasyetnotwell-understoodlogicthatplaysabasicroleinwhatmaywellbeoneofthemostimportantfacetsofhumanthinking,namely,theabilitytosummarizeinformation-toextractfromthecollectionofmassesofdataimpinginguponthehumanbrainthoseandonlythosesubcollectionswhicharerelevanttotheperformanceofthetaskathand.4.5.3FuzzyControllersFuzzycontrollersareaspecialtypeofexpertsystemthatsolvesreal-timecontrolproblemsviatheapplicationoffuzzyrules.Theseareofspecialinterestwhentheproblemisdifculttoconstructmodelsfororwhenthesemodelsarecomputationallyexpensiveordifculttouse.Itcanbeeasiertoconstructandcontrolthissystemwithasetofrulesdenedbyfuzzypropositions.Thesepropositionsaregivenasif-thenrulesusingnaturallanguage 50

PAGE 63

IFthereislittlecloudcover//ANDthedayiswarm//THENtherewillbemanypeopleonthebeachandthesevaguetermscanbeeasilyrepresentedbyfuzzysetsovertheirindividualuniversesofdiscourse,namelycloudcover,temperature,andpeopleonthebeach.Atypicalfuzzycontrollerconsistsofasetoffuzzyrules,afuzzyinferenceengine,afuzzicationmoduleandadefuzzicationmethod.Thesecontrollerscanbeusedtoperformsensorfusionbythemselvesor,moretypically,areusedtocontrolpartsofanothersensorfusionorlocalizationmethod.Acontrollercyclesthrougheachpartinorder,usingthefuzzyrulestoprovideacontrolactionoralteringavariable,asshowninFigure7.Itstartsbyfuzzifyingtheinputconditionsviathefuzzicationmodule,changingthemfromtheircrispvaluestofuzzyones.Thisisdonebyexpressingtheuncertaintyintheinputconditionsasafuzzicationfactoroverthesetofcrispvalues.Thisresultsinafuzzysetorfuzzysetsdenedovertheirindividualuniversesofdiscourse.Thesefuzzyvaluesarethenputintothefuzzyinferenceengine,whichoutputsasetorsetsoffuzzyoutputsbasedonthesetoffuzzyrules.Thesefuzzyoutputshavebeentransformedbasedonthoserulesandarethenrunintoadefuzzicationmethodinordertoextractcrispoutputactions.ThisiscoveredmoreindepthbyDing,Ma,andKandel,aswellasbyLi,Cao,andKandelDing,Ma,andKandel2000;Kandel,Li,andCao1992.4.5.3.1DefuzzicationMethodsThereareseveraldifferentmethodsofdefuzzifyingvariables,allwithdifferentfocusesandresults.ThecenterofareaCOAsometimesknownasthecentroidorcenterofgravitymethoddenesthedefuzziedvaluedCACasthevaluewithintherangeofvariablevwheretheareaunderthegraphofC,themembershipfunction,thatdividesCintotwoequalsubareas:dCAC=RC)]TJ/F10 6.9738 Tf 6.2267 0 Td[(CCzzdz RC)]TJ/F10 6.9738 Tf 6.2267 0 Td[(CCzdzInadiscreteuniverse,whereCisdenedoverfz1;z2;:::;zngthisbecomes:dCAC=Pnk=1Czkzk Pnk=1CzkandifdCAChasavaluenotfoundinfz1;z2;:::;zng,choosethevalueclosesttoitLee1990bKlirandYuan1995.ThemeanofmaximummethodMOMisusuallyadiscretemethod,theaverageofallvalues 51

PAGE 64

Figure7.OverviewofaFuzzyController 52

PAGE 65

ofsetM,whereMisequalto:M=fzkjCzk=hCgandhCistheheightofC.Formally,thisisrepresentedas:dMMC=Pzk2Mzk jMjLee1990bKlirandYuan1995.ThecenterofmaximamethodCOMtakestheaverageofthesmallestvalueandlargestvalueforwhichMinEquationistrue:dCMC=minfzkjzk2Mg+maxfzkjzk2Mg 2Thismethodalsohasacontinuouscase,whereM=fz2[)]TJ/F11 9.9626 Tf 7.7487 0 Td[(c;c]jCz=hCg:dCMC=infM+supM 2ThisotherMcanbeusedfortheMOMmethod,aweightedaverageofthemeansoftheintervalsKlirandYuan1995.Theproduct-sum-gravityPSGmethodisliketheCOAmethod,exceptthatitreplacestheintegralswithareasoftriangles,centeredonthemidpointofv,withheightdenedbytheinputintothedefuzzicationmethod.Siistheareaofthetrianglecenteredonvwithheighth:dPSGC=S1z1+S2z2++Snzn S1+S2++SndPSGCisthereforedenedastheweightedaverageofziwithweightsSiMizumoto1994.AllofthesemethodscanbederivedfromaparameterizedmethodbyFilevandYagercalledBADDorBasicDefuzzicationDistributionFilevandYager1991.Theequationgivenbelowcanbealteredtodefuzzifydataasanyoftheabovemethods,andthesameparameterservesasacondencemetricforthecontrollerasawhole.ItisbasedonamappingfrompossibilitytoprobabilitydistributionsgivenbyKlir-vi=kwi;>0;i=1;n-wherewiisthedegreeofpossibilityandviisthedegreeofprobabilityforelementfui2UgKlir1990.AssumeafuzzysetFwithapossibilitydistributionwi=Fui;i=1;n,whereuiisthecontrolinput.ThedefuzziedvalueofFshouldbetheexpectedvalueoftheBAD 53

PAGE 66

distributionasshown:dBADD=Pni=1wiui Pni=1wiwhere:vi=wi Pni=1wi;i=1;nThederivationsofthisequationaswellastheconversiontotheothermethodsarefoundinFilevandYager1991.Ifislow,itreectsalowcondencelevelinthefuzzyruleswhileahighrepresentsahighcondencelevel.=0iszerocondence,whichmakesdBADDtheaveragewithzeroweighting,while!1becomestheMOMmethodasinEquation.4.5.4FuzzyOptimalityBytheirnature,fuzzylogicsystemsareuncertain.Whyshouldoneusethemforacontrollerthen?Whilesomeofthereasonsaboveservetoamelioratetheseconcerns,anotherextremelyimportantreasonisthatfuzzysystemsareuniversalapproximatorsKosko1994-whichhasaprecisemathematicaldenitionfromKlirandYuanKlirandYuan1995: LetXbeacompactsubsetofRni.e.,asubsetofRnthatisclosedandbounded,andletCX;n;mdenotethesetofallcontinuousfunctionsfoftheformf:X!Rm.Thenauniversalapproximator,A,isasetoffunctionsgoftheformg:X!Rmthatsatisesthefollowing:foranygivenfunctionf2CX;n;mandanyrealnumber">0,thereexistsafunctiong2Asuchthatjfx)]TJ/F11 9.9626 Tf 9.9626 0 Td[(gxj<"forallx2X.WhileAisusuallyasubsetofCX;n;m,thetwosetsmayalsobedisjoint.ThelattercasetakesplacewhenfunctionsinAarenotcontinuous.Inshort,thismeansthatafuzzysystemcanapproximateanyfunctionwithanerrorlessthan".Thisisofpracticaluseasfeedforwardneuralnetworkswithasinglehiddenlayerandacontinuousactivationfunctionareanotheruniversalapproximator,andthereforeeachcanapproximatetheother.Hence,giventherightkindofhardware,fuzzyrulescanbecomputedinamassivelyparallelfashion,usinganeuralnetworkbacking. 54

PAGE 67

4.6ConclusionsThewidevarietyofmathematicalltersthatareapplicabletosensorfusionandrobotlocalizationisstaggering.Controllersofalltypes,especiallyfuzzylogiccontrollers,addtothecomplexityandpossibleaccuracyofthesystems-fuzzylogic'sabilitytoactasauniversalapproximatorandtolimitourareaofdiscoursetoareasonablelevelafrmsitsroleandnecessitytobetestedinthenextchapter.Forsmall,palm-toprobots,thediscussionofthealgorithmsisimportant,asithelpstofurtherdenetheproperchoiceforrobotlocalizationgiventheirlowcomputationalpoweranduncertainsensors.TheSPKFisderivativefree,akeybenetforsmallchipsandhardwarethatlacksadedicatedderivativesolver.Fuzzylogicsimpliescontrollerdesignandlogic,especiallywiththeadditionofadedicatedfuzzylogicchip.TheEKFisbyfarthemostsimple,makingitanothergoodcandidate. 55

PAGE 68

Chapter5Results 5.1ExperimentalDesignAstherewasalackofgooddatacontainingGPSdisruption,asimulationenvironmentwasderivedfromtheabilitiesofthesensorsmountedontherobotitself.TheGPSwasmodeledwithrandomerrorofupto1.5metersandasensorupdaterateof40milliseconds.TheIMUwasmodeledwithadriftofupto0.1176m=s2andasamplerateof10milliseconds.Thecompasswasmodeledwithanerrorofupto0.8radsRMSandasamplerateof125milliseconds.Theodometrywaswithanerrorof0.1millimetersperupdateandanupdaterateasfastasthefastestsensormilliseconds.Allsensorshada1/100000chanceofamajorerror-onethatwouldprovideextremelybadvalues.Therobotsimulationwasdrivenatarateof2m=sandturnsatarateof0.1rads=s.TheoutputofthisisshowninFigure8a.Thesecondsimulationusedthesamesensorsetasabove,withthesamerates,andaddedabridgeoralocationwheretheGPSwouldreceivemultipatherrors.Thesemultipatherrorsweremodeledasarandomerroruptothebridgewidth,or10meters,andoccurredonlyduringtheocclusionperiod-between70and80meters.ThisisshowninFigure8b.TheGPSoutputfortheBridgedatasetisshowninFigure9-themultipatherrorisclearlyvisibleandwouldresultinalossofcontrolfortherobotrunningonlyonGPS.Athirdsimulationinvolves20randompointsina1squarekilometerarea.Thissimulationrunsforfarlongerandhasalargeroccludedarea-from350to400metersdowntheentireNorth-Southaxis.Therobotinthissimulationhasasimplemodiedbangbangdrivingstyle,withagoodenoughmetricfordetermininglocation.Thismodelshowareasonablyaccuratehumandriverwouldoperatetherobot-theyturntherobottofacethelocationinquestion,thendriveinastraightline.Theywouldthenslowdownastheyapproachwithinameterofthetargetarea,stoppingshortlyoutsideofit.Thetruerobotpathforthe20PointdatasetisinFigure10andtheGPSoutputisshowninFigure11.ThecodeforthesimulationgenerationiswritteninJavaandisincludedinAppendixB.Astherobot'ssensorsetisextremelyovercontrained,everysensorisfusedwithanotherinordertocreatethebestsetofvalues.Thismakesthenalsensorfusioninthecascadethemostimportantfor 56

PAGE 69

determiningthenaloutput.Thisdesigncharacteristicwillbediscussedineachsection,aswellastheissuesthatarisefromit.ThecodeiswritteninJavaandisincludedinAppendixA. aStandardDataset bBridgeDataset Figure8.TheTrueRobotPathfortheStandardandBridgeDatasets 5.2ExtendedKalmanFilter5.2.1DesignAlloftheKalmanltersusethesamebasiccascadedesign,asshowninFigure12.EachfusionstepisunderOk3-withtheoverallsystemusingameasurementvectorofatmostk=12.5.2.2ResultsTheEKFprovidedsurprisinglygoodresultsforboththeStandardandBridgedatasets,asshowninFigure13.Theresultsweresimilarlyacceptableinthe20PointdatasetrepresentedinFigure14.TheresultsaregiveninTable13. Table13.ResultsforEKF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 286.2 1.303 1.291 1.066 1.059 Bridge 289.8 2.360 2.443 1.564 1.617 20Point 10445.4 6.236 5.837 2.264 2.246 57

PAGE 70

Figure9.TheGPSOutputfortheBridgeDataset Figure10.TheTrueRobotPathforthe20PointDataset 58

PAGE 71

Figure11.TheGPSOutputforthe20PointDataset Figure12.TheKalmanCascadeDesign 59

PAGE 72

aStandardDataset bBridgeDataset Figure13.TheEKFOutputfortheStandardandBridgeDataset Figure14.TheEKFOutputforthe20PointDataset 60

PAGE 73

5.3FuzzyEKF5.3.1DesignAsdeclaredbefore,allKalmanltersusethesamebasiccascadedesign,asshowninFigure12.ThefuzzylogicenginewithinJavajFuzzyLogicisextremelyslow,averaging0.3millisecondsperupdate.Thisaddedabout120secondstothenaltotaltimeelapsedandpusheditoutofrealtimerange.Withtheuseofadedicatedfuzzylogiccontroller,thistimecanbereduced10fold,bringingthesefuzzycontrolledsystemsbackintothepossiblesolutionrange.5.3.2ResultsByaddingthefuzzycontrollertotheEKF,thelterachievedsuperiorperformanceinboththeBridgeandStandarddatasets,byafactorofabout1.4.TheGPSandIMUerrorsdestroyedtheplantinthe20Pointdataset,disturbingitbeyondrecovery.TheseresultsarereportedinTable14.Providedonecanlocateafeasiblefuzzylogiccontrollerandeliminateerrors,theFEKFisasuperiorchoiceoverthebasicEKF. aStandardDataset bBridgeDataset Figure15.TheFEKFOutputfortheStandardandBridgeDatasets Table14.ResultsforFEKF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 160024.6 1.386 1.414 0.765 0.759 Bridge 159957.2 1.973 2.052 1.111 1.150 20Point 4728402 7.200 6.771 2.668 2.656 61

PAGE 74

Figure16.TheFEKFOutputforthe20PointDataset 5.4Sigma-PointKalmanFilter5.4.1DesignTheSPKFsharesthesamebasicdesignastheotherKalmanlters.Ithasaslightlydifferentinternaldatastoragescheme,butproceedsthroughthealgorithmsinmuchthesamefashion.Asaresult,thecascadedesignshowninFigure12stillholds.5.4.2ResultsTheSPKFprovidessuperiorresultsinboththeStandardandBridgedatasets,outstrippingallothers.Itisbyfarthebestchoiceamongstthecompetitors,asshowninTable15andFigures17a,17band18. Table15.ResultsforSPKF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 14681.6 0.816 0.802 0.671 0.662 Bridge 14534.6 1.519 1.596 0.986 1.024 20Point 518997.5 4.070 3.780 1.435 1.418 62

PAGE 75

aStandardDataset bBridgeDataset Figure17.TheSPKFOutputfortheStandardandBridgeDatasets Figure18.TheSPKFOutputforthe20PointDataset 63

PAGE 76

5.5FuzzySPKF5.5.1DesignTheFuzzySPKFaddsafuzzylogiccontroller,muchliketheoneaddedtotheEKFtocreatetheFEKF.TheFLACcontrolsthelterbyalteringtheQandRvaluesonthey:Qk=FLACQbase;Pk;)]TJ/F10 6.9738 Tf 11.2201 4.3835 Td[(kXi=1zi)]TJ/F11 9.9626 Tf 9.9626 0 Td[(x-i=k!Rk=FLACRbase;Pk;)]TJ/F10 6.9738 Tf 11.2201 4.3835 Td[(kXi=1zi)]TJ/F11 9.9626 Tf 9.9626 0 Td[(x-i=k!Asyoucansee,theseequationsareexactlythesameasthoseshownfortheFEKFinaprevioussection,namelyEquationsand.However,theFSPKFdiffersinthefuzzyvaluesassigned,asshowninFigure19comparedtoFigures3,4,and5. aFSPKFCovarianceFuzzyInput bFSPKFMeanFuzzyInput cDFSPKFAlphaFuzzyOutput Figure19.FuzzyControllerforQandR 64

PAGE 77

5.5.2ResultsTheFuzzySPKFperformedworsethanthestandardSPKF.Thisislikelyduetotheneedformorecarefultuningofthefuzzyparametersandthesystem'sinherentestimationcapabilities.TheseresultsareinTable16.Thisdemonstratesthatafuzzylogiccontrollerisnotapanacea-itmustbecarefullyappliedandtuned.Also,thecontrolleraddedabout120secondsofruntimetothealgorithmfortheStandardandBridgedatasets,aclearlyunacceptablenumber.Thiswillbereducedgreatlybytheadditionofadedicatedfuzzylogicchip. aStandardDataset bBridgeDataset Figure20.TheFSPKFOutputfortheStandardandBridgeDatasets Table16.ResultsforFSPKF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 137346.3 0.637 0.624 0.943 0.928 Bridge 137321.5 1.749 1.796 1.511 1.566 20Point 5218453 6.375 5.970 2.320 2.303 5.6DoubleFuzzySPKF5.6.1DesignTheDoubleFuzzySPKFDFSPKFaddsasecondfuzzylogiccontrollertotheSPKF,thistime,alteringtheandvaluesfortherstthreefusions,andchanginghowtheRxvalueiscalculatedforthenalfusion.TheplotsofthefuzzyvaluesareshowninFigures22and23.ThefuzzylogicdesignisalsoprovidedinTables17and18-itprovidesthefuzzyreasoningforeachchange.Deviationiscalculatedas 65

PAGE 78

Figure21.TheFSPKFOutputforthe20PointDataset thedifferencebetweenthepreviousGPSxplusthesinglestepIMUdistancechangeminusthecurrentGPSx.ThelterusesthesamecascadedesignastheotherKalmantypelters. Table17.FuzzyControllerDesignforDoubleFuzzySPKF-Controllingand AvgLow AvgMid AvgHigh CovarLow """ )]TJ/F11 9.9626 Tf 10.2394 0 Td[("" ## CovarMid "" #" #### CovarHigh #)]TJET1 0 0 1 327.4135 368.1457 cmq[]0 d0 J0.3985 w0.1992 0 m0.1992 11.9552 lSQ1 0 0 1 -327.4135 -368.1457 cmBT/F11 9.9626 Tf 333.7896 371.7323 Td[(### #### Table18.FuzzyControllerDesignforDoubleFuzzySPKF-ControllingRx DeviationLow Rx# DeviationMid Rx" DeviationHigh Rx"" 5.6.2ResultsTheDoubleFuzzysystemprovidednobenetoverthestandardSPKF.Alteringtheand,aswellastheRxandQxprovidedlittlebenet,whileaddingalargeamountoftimeabout200secondsfortheStandardandBridgedatasetsduetothecomputationalcomplexityofthefuzzylogicsystem.TheresultsofthissystemareprovidedinTable19.Whilethisparticularexperimentwasafailure,futureexperimentsshouldprovidefarbetterresults. 66

PAGE 79

aDFSPKFCovarianceFuzzyInput bDFSPKFMeanFuzzyInput cDFSPKFAlphaFuzzyOutput dDFSPKFBetaFuzzyOutput Figure22.DoubleFuzzyControllerforand aDFSPKFDeviationFuzzyInput bDFSPKFgmodFuzzyOutput Figure23.DoubleFuzzyControllerforRx 67

PAGE 80

aStandardDataset bBridgeDataset Figure24.TheDFSPKFOutputfortheStandardandBridgeDatasets Figure25.TheDFSPKFOutputforthe20PointDataset Table19.ResultsforDFSPKF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 211777 0.667 0.651 1.077 1.068 Bridge 211519.5 1.747 1.809 1.579 1.634 20Point 8017771 6.280 5.878 2.288 2.270 68

PAGE 81

5.7SISParticleFilter5.7.1DesignTheSISParticleFilteristhebasicparticlelterdesign.BothparticleltersusetheKLDSampling,MeanDensityExtraction,andmixtureparticles%.Thiscompromisesthemostrobustandfastestmethodscurrentlyavailable.5.7.2ResultsTheSISPFhandledtheStandarddatasetpoorlyasshowninFigure26a,whiletheBridgedatasetshowedanimprovementoverbaselineGPSinFigure26b.Thisisduetotheadditionofmixtureparticles,aswellastheKLDSamplinggrowingandshrinkingtheparticleset-inasimpleenvironment,itaddsmorenoisethanitcanreduce,whilethemorecomplexbridgeenvironmentismoreaccuratelydecoded.Thelongdistancerunsofnoocclusionagainmadethelterfarepoorlyinthe20Pointdataset,asshowninFigure27.Themeandensityextractionisalsotheleastaccuratemethod,butitisthefastest.Thisisquitenecessarygiventhelargenumberofparticlesinvolvedandtherelativescarcityofcomputationalresources-evenwiththetimesavingprocedures,theruntimewasfartoolong. aStandardDataset bBridgeDataset Figure26.TheSISPFOutputfortheStandardandBridgeDatasets Table20.ResultsforSISPF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 201572.3 2.373 2.281 3.389 3.163 Bridge 207094.8 1.885 2.035 2.600 2.755 20Point 7752004 7.848 5.026 7.229 3.550 69

PAGE 82

Figure27.TheSISPFOutputforthe20PointDataset 5.8SPKFParticleFilter5.8.1DesignTheSPKFParticleFilterusestheSPKFtoproposetheimportancedensity.Asstatedbefore,bothparticleltersusetheKLDSampling,MeanDensityExtraction,andmixtureparticles%.Thiscompromisesthemostrobustandfastestmethodcurrentlyavailable.5.8.2ResultsTheSPKFPFhandledtheStandarddatasetpoorlyasshowninFigure28a,whiletheBridgedatasetshowedanimprovementoverbaselineGPSinFigure28b.ItdidfarbetterinallwaysthantheSISPFforthe20Pointdataset,asshowninFigure29.Thisisduetotheadditionofmixtureparticles,aswellastheKLDSamplinggrowingandshrinkingtheparticleset-inasimpleenvironment,itaddsmorenoisethanitcanreduce,whilethemorecomplexbridgeenvironmentismoreaccuratelydecoded.Themeandensityextractionisalsotheleastaccuratemethod,butitisthefastest.Thisisquitenecessarygiventhelargenumberofparticlesinvolvedandtherelativescarcityofcomputationalresources-evenwiththetimesavingprocedures,theruntimewasfartoolong. 70

PAGE 83

aStandardDataset bBridgeDataset Figure28.TheSPKFPFOutputfortheStandardandBridgeDatasets Figure29.TheSPKFPFOutputforthe20PointDataset Table21.ResultsforSPKFPF Dataset TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters Standard 217061.7 2.431 2.343 3.400 3.201 Bridge 204329.8 1.924 1.859 2.651 2.574 SPKFPF 8022980 3.424 3.409 3.117 3.136 71

PAGE 84

5.9MultipleHypothesisMethodsThevariousmultiplehypothesismethodsallsharedthecharacteristicsoftheiroriginaldesigns,withthefollowingcaveats-theyalltookabout5timesaslonggiventhe5lterswithinandprovidednoormarginalgainsandlossesinmostcases.TheexceptiontothisistheMHFEKFandthe20Pointdataset-thislteroutperformedallothers,exceptinspeed.TheseresultsareinTable22,Table23,andTable24. Table22.MultipleHypothesisResultsfortheStandardDataset Filter TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters MHEKF 1764 1.303 1.291 1.066 1.059 MHFEKF 803680.8 1.410 1.396 0.764 0.753 MHSPKF 72739 0.977 0.964 0.803 0.797 MHFSPKF 670522.5 1.277 1.263 1.086 1.079 Table23.MultipleHypothesisResultsfortheBridgeDataset Filter TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters MHEKF 1764 2.360 2.443 1.564 1.617 MHFEKF 802855.8 1.987 2.048 1.114 1.147 MHSPKF 73889 1.802 1.899 1.180 1.223 MHFSPKF 669869.2 2.386 2.470 1.598 1.653 Table24.MultipleHypothesisResultsforthe20PointDataset Filter TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters MHEKF 60501 6.236 5.837 2.264 2.246 MHFEKF 29825326 1.987 2.048 1.114 1.147 MHSPKF 2641112 4.070 3.780 1.485 1.438 72

PAGE 85

aTheMHEKFOutputfortheStandardDataset bTheMHEKFOutputfortheBridgeDataset cTheMHEKFOutputforthe20PointDataset Figure30.TheMHEKFResults 73

PAGE 86

aTheMHFEKFOutputfortheStandardDataset bTheMHFEKFOutputfortheBridgeDataset cTheMHFEKFOutputforthe20PointDataset Figure31.TheMHFEKFResults 74

PAGE 87

aTheMHSPKFOutputfortheStandardDataset bTheMHSPKFOutputfortheBridgeDataset cTheMHSPKFOutputforthe20PointDataset Figure32.TheMHSPKFResults 75

PAGE 88

aTheMHFSPKFOutputfortheStandardDataset bTheMHFSPKFOutputfortheBridgeDataset Figure33.TheMHFSPKFResults 76

PAGE 89

5.10ComparisonsThedatashowsthattheSPKFistheclearchoiceinallthedatasets-thisisshowninTables25and26,withtheFEKFbeinganothergoodchoicefortheStandardandBridgedatasets.TheSPKFPFisagoodchoiceforthelongerandmorerandom20Pointdataset,aswellastheMHFEKFmakinganextremelystrongshowing.TheFEKFandMHFEKFwouldbeevenbetterwithamuchfasterfuzzylogicengine.ThecomparisonbetweenthetwoltersandtherawGPSareprovidedgraphicallyfortheBridgedatasetinFigures34,35and36,andforthe20PointdatasetinFigures37,38,and39. Table25.StandardDatasetResults FilterType TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters GPS 100000 1.448 1.432 1.254 1.242 EKF 286.2 1.303 1.291 1.066 1.059 FEKF 160024.6 1.386 1.414 0.765 0.759 SPKF 14681.6 0.816 0.802 0.671 0.662 FSPKF 137346.3 0.637 0.624 0.943 0.928 DFSPKF 211777 0.667 0.651 1.077 1.068 SISPF 201572.3 2.373 2.281 3.389 3.163 SPKFPF 217061.7 2.431 2.343 3.400 3.201 MHEKF 1764 1.303 1.291 1.066 1.059 MHFEKF 803680.8 1.410 1.396 0.764 0.753 MHSPKF 72739 0.977 0.964 0.803 0.797 MHFSPKF 670522.5 1.277 1.263 1.086 1.079 Table26.BridgeDatasetResults FilterType TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters GPS 100000 2.703 2.784 1.839 1.901 EKF 289.8 2.360 2.443 1.564 1.617 FEKF 159957.2 1.973 2.052 1.111 1.150 SPKF 14534.6 1.519 1.596 0.986 1.024 FSPKF 137321.5 1.749 1.796 1.511 1.566 DFSPKF 211519.5 1.747 1.809 1.579 1.634 SISPF 207094.8 1.885 2.035 2.600 2.755 SPKFPF 204329.8 1.924 1.859 2.651 2.574 MHEKF 1764 2.360 2.443 1.564 1.617 MHFEKF 802855.8 1.987 2.048 1.114 1.147 MHSPKF 73889 1.802 1.899 1.180 1.223 MHFSPKF 669869.2 2.386 2.470 1.598 1.653 77

PAGE 90

Table27.20PointDatasetResults FilterType TimeElapsed Std.Dev.X Std.Dev.Y Avg.ErrorX Avg.ErrorY ms meters meters meters meters GPS 3801860 7.189 6.758 2.667 2.655 EKF 10445.4 6.236 5.837 2.264 2.246 FEKF 4728402 7.200 6.771 2.668 2.656 SPKF 518997.5 4.070 3.780 1.435 1.418 FSPKF 5218453 6.375 5.970 2.320 2.303 DFSPKF 8017771 6.280 5.878 2.288 2.270 SISPF 7752004 7.848 5.026 7.229 3.550 SPKFPF 8022980 3.424 3.409 3.117 3.136 MHEKF 60501 6.236 5.837 2.264 2.246 MHFEKF 29825326 1.987 2.048 1.114 1.147 MHSPKF 2641112 4.070 3.780 1.485 1.438 Figure34.TheSPKFOutputfortheBridgeDataset 78

PAGE 91

Figure35.TheFEKFOutputfortheBridgeDataset Figure36.TheGPSOutputfortheBridgeDataset 79

PAGE 92

Figure37.TheSPKFOutputforthe20PointDataset Figure38.TheMHFEKFOutputforthe20PointDataset 80

PAGE 93

Figure39.TheGPSOutputforthe20PointDataset 81

PAGE 94

Chapter6Conclusions Withoutlocalization,arobotishopelesslylostinanenvironmentitcannotunderstand.Withnoreferencepointinourworld,arobotcannotdoanyusefulwork.Althoughtheyareckle,pronetoerrors,andnoisydata,aswellasfailures,sensorsarethetypicalmethodofperforminglocalization.Ifthesesensorswereperfect-neverfailedorreturnederroneousvalues-thelocalizationproblemwouldbesolved.However,sensorsaretypicallycomplexmechanicalandelectronicdevicesthatmustcollapseaninnitenumberofvariablesintheactualworldintoasinglesetofvaluesfortherobottoconsume.Thistaskismademoredifcultbyoutsideinterference,suchasincidentsunlight,magneticelds,andmovinghumansandvehicles.Anindividualsensoroftentimesdoesnotprovideawholeoraccuratepicture.Sensorfusionistheprocessofcombiningsensordatainordertoeliminateerrors.Thesimplestmethodofsensorfusionwouldbeakintotakingtwohalvesofamapandmatchinguptheedges-justllinginforwhat'smissingfromtheothersensororsensors.However,thesimplemethodfailswhenonerealizesthatmapscouldbemisaligned,misprintedorjustwrong-awayisneededtojudgethepotentialaccuracyoftheestimatedlocalization,theaccuracyofthesensors,andcometoabest-casebeliefofwheretherobotcurrentlyis,allinrealtime.Inordertoachievethisgoal,oneturnstomathematicallters,orrecursivemethodstocalculatebelief.Mathematicalltersprovidethebestrecoursetosolvingthisproblem,asthey,ingeneral,recursivelysolvetheleast-squaresproblemofGauss-activelymovingtowardsamoreaccuratesolutionastheamountofresourcesspentontheproblemincrease.Thisself-correctingandhill-climbingbehaviorisextremelyusefulinsolvingthelocalizationproblem-thebestcaseisyourmostcurrentone,givenyourmostrecentsensormeasurements.6.1SummaryWesetouttocompletetwodifferentintertwinedgoals.Therstwastoestablishanoverviewofallavailablesensorfusionmethods,discussingtheirdesign,theirdrawbacks,assumptionsandbenets.Thesecondwastochooseanalgorithmthatisoflowcomputationalcomplexity
PAGE 95

equalinormoreaccuratethanadifferentialGPSDGPSgivenlowerqualitysensors,androbust-abletoprovideausefullocalizationsolution,evenwhensensorsarefaultyorinaccurate.Thegoalwastondalocusbetweenpowerrequirements,computationalcomplexityandchiprequirementsandaccuracy/robustnessthatprovidesthebestofbreedforsmallrobotswithinaccuratesensors.TherstwascompletedinChapters2,3,and4.ThesecondwasrevealedinChapter5withaselectionofnewlterdesigns,anditwasdemonstratedthattheSigma-PointKalmanFilterwastheclearwinner,withtheadaptiveFuzzyEKFaclosesecond.Theoverviewofdifferentsensorfusionapproacheswillhelpresearchersstopthetendencytoready-re-aim-theywillbeabletoconsideralloftheoptionsofeachalgorithm,weightthedrawbacks,assumptions,andbenetsbeforechoosingoneormodifyingonetotheirneeds.Theresultsofthisstudywillhelppickthemostappropriatesensorfusionalgorithmgivenrelativelyrestrictiverequirements.Theserequirementsaresimilartothosethatamicrorobotwouldhavewithextremelylimitedpayloadcapacity,power,andcomputationalresourcesavailable.TheSigma-PointKalmanFilterSPKFwastheclearwinner-usingonlyabout10%oftheavailabletimeresourceswhilebeinglightweightandhighlyeffectivegivenpoorsensors,evenunderextremeinterruptionithas,onaverage,submeteraccuracyevenwithinterruption.TheFuzzyEKFFEKFandMultipleHypothesisFEKFwereaclosesecond,almostmatchingtheSPKFintermsofaccuracyandrobustness,whilelosingoutinthecomputationalcost-somethingthatwillbealleviatedbytheadditionofadedicatedfuzzyprocessor.6.2FutureWorkWhiletheideasbehindsensorfusionarerelativelymature,thereisalwaysroomformoreinnovation.Therenoresearchprojecthasaddressedhowtochoosethebestvaluesfor,andforSPKFs.Theuseoffuzzylogictohelpcontrolthesesystemsisstillattimesablackart-theproperchoicesforthefuzzysetsstillrequiresgoodguessingandhandtuning.Thereisalargeamountoffutureresearchthatcanbedoneinfuzzysystemsthemselves-removingalldefuzzicationfromthesystem-fuzzyin,fuzzyout.Also,thereismuchtobedoneincategorizingsensorswithtype2fuzzysets-fuzzysetsthatarethemselvesfuzzy.Ifasensorcanbecategorizedwithatype2setlikethis,itwouldbeanextremelylightweightprocesstoperformsensorfusionandtocontrolarobotoranyothersystemwithit.Finally,particleswarmsofferaninterestingpossibilityforsuperintelligentparticlessolvingtheirownlocalizationproblems,activelyseekingthelowestresidualsovertheentirepossibledesignspace. 83

PAGE 96

References Abdelnour,G.,S.Chand,S.Chiu,andT.Kido.1993.On-linedetectionandcorrectionofKalmanlterdivergencebyfuzzylogic.Proceedingsofthe1993AmericanControlConference.1835. Anderson,B.D.O.1973.ExponentialDataWeightingintheKalman-BucyFilter.InformationSciences5:217. Armesto,L.,andJ.Tornero.2004.SLAMbasedonKalmanlterformulti-ratefusionoflaserandencodermeasurements.ProceedingsofIntelligentRobotsandSystemsIROS2September:1860. Arras,K.O.,N.Tomatis,B.T.Jensen,andR.Siegwart.2001.Multisensoron-the-ylocalization:Preci-sionandreliabilityforapplications.RoboticsandAutonomousSystems34:131. Arulampalam,M.,S.Maskell,N.Gordon,andT.Clapp.2002.ATutorialonParticleFiltersforOnlineNonlinear/Non-GaussianBayesianTracking.IEEETransactionsonSignalProcessing50:174February. Asadian,Ali,BehzadMoshiri,andAliKhakiSedigh.2005,October.ANovelDataFusionApproachinanIntegratedGPS/INSSystemUsingAdaptiveFuzzyParticleFilter.5thInternationalConferenceonTechnologyandAutomationICTA.IEEEandEURASIP,Thessaloniki,Greece. Ashokaraj,I.,A.Tsourdos,P.Silson,B.White,andJ.Economou.2004,June.AfuzzylogicapproachinfeaturebasedrobotnavigationusingintervalanalysisandUKF.IEEEAnnualMeetingoftheNorthAmericanFuzzyInformationProcessingSociety,Volume2.NAFIPS,Banff,Alberta,Canada:IEEE,808. Barshan,B.,andH.F.Durrant-Whyte.Jun1995.Inertialnavigationsystemsformobilerobots.RoboticsandAutomation,IEEETransactionson11:328. Berzuini,Carlo,NicolaG.Best,WalterR.Gilks,andCristianaLarizza.1997.DynamicConditionalIndependenceModelsandMarkovChainMonteCarloMethods.JournaloftheAmericanStatisticalAssociation92:1403. Borenstein,J.,B.Everett,andL.Feng.1996.NavigatingMobileRobots:SystemsandTechniques.Welles-ley,MA:A.K.Peters,Ltd. Burgard,W.,A.Derr,D.Fox,andA.B.Cremers.1998.Integratingglobalpositionestimationandpositiontrackingformobilerobots:theDynamicMarkovLocalizationapproach.ProceedingsoftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystemsIROS,Volume2.730. Cappelle,Cindy,MaanElBadaouiElNajjar,DenisPomorski,andFrancoisCharpillet.2007,June.Locali-sationinurbanenvironmentusingGPSandINSaidedbymonocularvisionsystemand3Dgeographicalmodel.Proceedingsofthe2007IEEEIntelligentVehiclesSymposium.IEEE,Istanbul,Turkey,811816. Cox,I.J.1991.Blanche-Anexperimentinguidanceandnavigationofanautonomousrobotvehicle.IEEETransactionsonRoboticsandAutomation7:193April. Cox,I.J.,andJ.J.Leonard.1994.Modelingadynamicenviromentusingabayesianmultiplehypothesisapproach.ArticialIntelligence66:311April. 84

PAGE 97

Crowley,J.1989,May.Worldmodelingandpositionestimationforamobilerobotusingultrasonicranging.ProceedingsoftheIEEEInternationalConferenceonRoboticsandAutomation.Scottsdale,AZ,674. Ding,Zouhua,MingMa,andAbrahamKandel.2000.Ontheobservabilityoffuzzydynamicalcontrolsystems.FuzzySetsandSystems111:225April. Doucet,A.1998.OnsequentialMonteCarlomethodsforBayesianltering.TechnicalReport,Depart-mentofEngineering,UniversityofCambridge,UK. Doucet,A.,N.deFreitas,K.Murphy,andS.Russell.2000.RaoBlackwellisedparticlelteringfordynamicBayesiannetworks.UncertaintyinArticialIntelligence. Doucet,A.,N.Gordon,andC.Andrieu.2000.OnsequentialMonteCarlosamplingmethodsforBayesianltering.StatisticalComputing10:197. Doucet,A.,N.J.Gordon,andV.Krishnamurthy.2001b.ParticleltersforstateestimationofjumpMarkovlinearsystems.IEEETransactionsonSignalProcessing49:613March. Duan,Zhuohua,ZixingCai,andJinxiaYu.2007.RobustPositionTrackingforMobileRobotswithAdaptiveEvolutionaryParticleFilter.ThirdInternationalConferenceonNaturalComputation.IEEE. Efraimidis,PavlosS.,andPaulG.Spiraki.2008.WeightedRandomSampling.EditedbyMing-YangKao,EncyclopediaofAlgorithms.Springer. Filev,D.,andR.R.Yager.1991.AgeneralizeddefuzzicationmethodunderBADdistributions.Interna-tionalJournalofIntelligentSystems6:687. Fox,D.2003.AdaptingthesamplesizeinparticleltersthroughKLD-sampling.InternationalJournalofRoboticsResearch22:985. Fox,D.,W.Burgard,F.Dellaert,andS.Thrun.1999.Montecarlolocalization:Efcientpositionesti-mationformobilerobots.ProceedingsoftheSixteenthNationalConferenceonArticialIntelligence.AAAI. Gordon,N.,D.Salmond,andC.Ewing.1995.Bayesianstateestimationfortrackingandguidanceusingthebootstraplter.JournalofGuidance,Control,andDynamics18:1434. Grisetti,Giorgio,CyrillStachniss,andWolframBurgard.2007.ImprovedTechniquesforGridMappingWithRao-BlackwellizedParticleFilters.IEEETransactionsonrobotics23:34February. Gutmann,J.-S.,andD.Fox.2002.AnExperimentalComparisonofLocalizationMethodsContinued.ProceedingsoftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystemsIROS. Hsu,J.C.2000,July.SelfLocationofanAutonomousMobileRobotUsingFuzzyAdaptiveExtendedInformationFilteringSchemes.Master'sthesis,DepartmentofElectricalEngineering,NationalChung-HsingUniversity. Isard,Michael,andAndrewBlake.1998.Condensationconditionaldensitypropagationforvisualtracking.InternationalJournalofComputerVision29:5. Jazwinski,A.H.1969.AdaptiveFiltering.Automatica5:475. Jensfelt,P.,andS.Kristensen.2001.Activegloballocalisationforamobilerobotusingmultiplehypothesistracking.IEEETransactionsonRoboticsandAutomation17:748. Julier,S.J.,andJ.K.Uhlmann.1997.ANewExtensionoftheKalmanFiltertoNonlinearSystems.InternationalSymposiumonAerospace/DefenseSensing,SimulationandControl.Orlando,FL. .2002.Reducedsigmapointltersforthepropagationofmeansandcovariancesthroughnonlineartransformations.AmericanControlConference,2002.Proceedingsofthe20022:887vol.2. .2004,March.UnscentedFilteringandNonlinearEstimation.ProceedingsoftheIEEE,Vol-ume92.IEEE,401. 85

PAGE 98

Kalman,R.E.1960.Anewapproachtolinearlteringandpredictionproblems.TransactionsoftheASME-JournalofBasicEngineering82:35. Kanazawa,K.,D.Koller,andS.Russell.1995.StochasticSimulationAlgorithmsforDynamicProbabilis-ticNetworks.Proc.UAI. Kandel,Abraham,LihongLi,andZhiqiangCao.1992.Fuzzyinferenceanditsapplicabilitytocontrolsystems.FuzzySetsandSystems48:99. Kennedy,J.,andR.Eberhart.Nov/Dec1995.Particleswarmoptimization.NeuralNetworks,1995.Proceedings.,IEEEInternationalConferenceon4:1942vol.4. Kim,Young-Joong,Chan-HeeWon,Jung-MinPak,andMyo-TaegLim.2007.FuzzyAdaptiveParticleFilterforLocalizationofaMobileRobot.InKnowledge-BasedIntelligentInformationandEngineeringSystems,Volume4694,41.SpringerBerlin/Heidelberg. Klir,GeorgeJ.1990.Aprincipleofuncertaintyandinformationinvariance.InternationalJournalGeneralSystems17:249. Klir,GeorgeJ.,andBoYuan.1995.FuzzySetsandFuzzyLogic.PrenticeHallPTR. Kobayashi,Kazuyuki,KaC.Cheok,KajiroWatanabe,andFumioMunekata.1998.AccurateDifferentialGlobalPositioningSystemviaFuzzyLogicKalmanFilterSensorFusionTechnique.IEEETransac-tionsonIndustrialElectronics45:510June. Kosko,B.Nov1994.Fuzzysystemsasuniversalapproximators.Computers,IEEETransactionson43:1329. Kwok,C.T.,D.Fox,andM.Meila.2004.Real-timeparticlelters.ProceedingsoftheIEEE92:469. .Mar/Apr1990b.Fuzzylogicincontrolsystems:fuzzylogiccontroller.II.Systems,ManandCybernetics,IEEETransactionson20:419. Lenser,S.,andM.Veloso.2000.SensorResettingLocalizationforPoorlyModelledMobileRobots.ProceedingsoftheInternationalConferenceonRoboticsandAutomationICRA,Volume2.12251232. Lin,Hung-Hsing,andChing-ChihTsai.2007.Improvedgloballocalizationofanindoormobilerobotviafuzzyextendedinformationltering.Robotica,pp.1. Lin,Hung-Hsing,Ching-ChihTsai,Jui-ChengHsu,andChih-FuChang.2003,September.UltrasonicSelf-LocalizationandPoseTrackingofanAutonomousMobileRobotViaFuzzyAdaptiveExtendedInformationFiltering.Proceedingsofthe2003IEEEInternationalConferenceonRoboticsandAu-tomation.IEEE,Taipei,Taiwan,1283. Liu,J.S.,andR.Chen.1998.SequentialMonteCarlomethodsfordynamicalsystems.JournaloftheAmericanStatisticalAssociation93:1032. Liu,Yang,FengchiSun,TongTao,JingYuan,andChaoLi.2007,August.ASolutiontoActiveSimul-taneousLocalizationandMappingProblemBasedonOptimalControl.Proceedingsofthe2007IEEEInternationalConferenceonMechatronicsandAutomation.IEEE,Harbin,China,314. Livchitz,Michael,AbrahamAbershitz,UriSoudak,andAbrahamKandel.1998.Developmentofanautomatedfuzzy-logic-basedexpertsystemforunmannedlanding.FuzzySetsandSystems93:145January. Marinakis,Dimitri,DavidMeger,IoannisRekleitis,andGregoryDudek.2007.HybridInferenceforSensorNetworkLocalizationusingaMobileRobot.AmericanAssociationforArticalIntelligence. Martinez-Cantin,Ruben,NandodeFreitas,andJoseA.Castellanos.2007,April.AnalysisofParticleMethodsforSimultaneousRobotLocalizationandMappingandaNewAlgorithm:Marginal-SLAM.IEEEInternationalConferenceonRoboticsandAutomation.IEEE,Roma,Italy:IEEE. 86

PAGE 99

Maslov,IgorV.,andIzidorGertner.2006.Multi-sensorfusion:anEvolutionaryalgorithmapproach.InformationFusion7:304September. Maybeck,PeterS.1979.StochasticModels,EstimationandControl.Volume1.AcademicPress. Mehra,R.K.1970.OntheidenticationofvariancesandadaptiveKalmanltering.IEEETransactionsonAutomaticControl15:175April. Metropolis,Nicholas,andS.Ulam.1949.TheMonteCarloMethod.JournaloftheAmericanStatisticalAssociation44:335September. Mizumoto,M.1994.FuzzyControlsUnderProduct-Sum-GravityMethodsandNewFuzzyControlMethods.Chapter13ofFuzzyControlSystems,editedbyAbrahamKandelandGideonLangholz,276.CRCPress. Mohamed,H.,andK.P.Schwarz.1999.AdaptiveKalmanFilteringforINS/GPS.JournalofGeodesy73:193. Montemerlo,M.2003.FastSLAM:AFactoredSolutiontotheSimultaneousLocalizationandMappingProblemwithUnknownDataAssociation.Ph.D.diss.,RoboticsInstitute,CarnegieMellonUniversity. Montemerlo,Michael,SebastianThrun,DaphneKoller,andBenWegbreit.2003.FastSLAM2.0:AnImprovedParticleFilteringAlgorithmforSimultaneousLocalizationandMappingthatProvablyCon-verges.ProceedingsoftheIJCAI2003. Mourikis,AnastasiosI.,StergiosI.Roumeliotis,andJoelW.Burdick.2007.SC-KFMobileRobotLo-calization:AStochasticCloningKalmanFilterforProcessingRelative-StateMeasurements.IEEETransactionsonRobotics23:717August. Ngaard,M.,N.K.Poulsen,andO.Ravn.2000.Newdevelopmentsinstateestimationfornonlinearsystems.Automatica36:1627November. Pitt,M.K.,andN.Shephard.1997,September.Filteringviasimulation:auxillaryparticlelters.TechnicalReport,NufeldCollege,OxfordUniversity. Pratt,K.,andJ.Kramer.2007.SensorFusionforRobotNavigationusingaFuzzy-EKFwithWeightedCovariance.InternationalConferenceforAdvancedRobotics2007ICAR2007. Ramer,Arthur,andVladikKreinovich.1994.ChapterInformationComplexityandFuzzyControlofFuzzyControlSystems,editedbyAbrahamKandelandGideonLangholz,76.CRCPress. Reina,Giulio,AndresVargas,KeijiNagatani,andKazuyaYoshida.2007,September.AdaptiveKalmanFilteringforGPS-basedMobileRobotLocalization.Proceedingsofthe2007IEEEInternationalWork-shoponSafety,SecurityandRescueRobotics.IEEE,Rome,Italy. Reuter,J.2000.Mobilerobotself-localizationusingPDAB.ProceedingsoftheInternationalConferenceonRoboticsandAutomation,Volume4.3512. Roberts,J.M.,D.J.Mills,D.Charnley,andC.J.Harris.1994,June.ImprovedKalmanFilterInitialisationsusingNeurofuzzyEstimation.4thIEEInternationalConferenceonArticialNeuralNetworks.329334. Rodriguez-Losada,Diego,FernandoMatia,LuisPedraza,AgustinJimenez,andRamonGalan.2007.Con-sistencyofSLAM-EKFAlgorithmsforIndoorEnvironments.JournalofIntelligentRoboticSystems50:375. Roumeliotis,S.I.,andG.A.Bekey.2000.BayesianestimationandKalmanltering:Auniedframeworkformobilerobotlocalization.ProceedingsoftheInternationalConferenceonRoboticsandAutoma-tion,Volume3.2985. Roumeliotis,StergiosI.,andG.A.Bekey.1997,October.AnExtendedKalmanFilterforFrequentLocalandInfrequentGlobalSensorDataFusion.ProceedingsoftheSPIESensorFusionandDecentralizedControlinAutonomousRoboticSystems.SPIE,Pittsburgh,Pennsylvania,11. 87

PAGE 100

Rubin,DonaldB.1987.TheCalculationofPosteriorDistributionsbyDataAugmentation:Comment:ANoniterativeSampling/ImportanceResamplingAlternativetotheDataAugmentationAlgorithmforCreatingaFewImputationsWhenFractionsofMissingInformationAreModest:TheSIRAlgorithm.JournaloftheAmericanStatisticalAssociation82:543June. Safotti,A.1997.Theusesoffuzzylogicinautonomousrobotnavigation.SoftComputing1:180. Sanjuan,M.,A.Kandel,andC.A.Smith.2006.Designandimplementationofafuzzysupervisorforon-linecompensationofnonlinearities:Aninstabilityavoidancemodule.EngineeringApplicationsofArticialIntelligence19:323April. Sasiadek,J.Z.,andQ.Wang.1999,May.SensorFusionbasedonFuzzyKalmanFilteringforAutonomousRobotVehicle.Proceedingsofthe1999IEEEConferenceonRoboticsandAutomation.IEEE,Detroit,Michigan,2970. Schulz,D.,andD.Fox.2004.Bayesiancolorestimationforadaptivevision-basedrobotlocalization.ProceedingsoftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystemsIROS,Vol-ume2.1884. Shen,Ying,ZhongxiangZhu,andEnrongMao.18-21Aug.2007.Double-FuzzyKalmanFilterBasedonGPS/IMU/MVSensorFusionforTractorAutonomousGuidance.AutomationandLogistics,2007IEEEInternationalConferenceon,pp.61. Smith,R.,M.Self,andP.Cheeseman.1990.Estimatinguncertainspatialrelationshipsinrobotics.InAutonomousRobotVehicles,editedbyI.J.CoxandG.T.Wilfon,167.SpringerVerlag. Sorenson,H.W.1970.Least-squaresestimation:fromGausstoKalman.IEEESpectrum7July:63. Sukkarieh,S.,E.M.Nebot,andH.F.Durrant-Whyte.Jun1999.AhighintegrityIMU/GPSnavigationloopforautonomouslandvehicleapplications.RoboticsandAutomation,IEEETransactionson15:572. Thrun,S.,W.Burgard,andD.Fox.2005.ProbablisticRobotics.Cambridge,MA:MITPress. Thrun,S.,D.Fox,andW.Burgard.2000.Montecarlolocalizationwithmixtureproposaldistribution.ProceedingsoftheAAAINationalConferenceonArticialIntelligence.AAAI,Austin,TX. Tornero,LeopoldoArmestoJosep,andMarkusVincze.2007.FastEgo-motionEstimationwithMulti-rateFusionofInertialandVision.TheInternationalJournalofRoboticsResearch26:577June. vanderMerwe,R.,A.Doucet,J.F.G.deFreitas,andE.Wan.2000.Theunscentedparticlelter.AdvancesinNeuralInformationProcessingSystems,vol.13December. vanderMerwe,Rudolph.2004,April.Sigma-PointKalmanFiltersforProbabilisticInferenceinDynamicState-SpaceModels.Ph.D.diss.,OGISchoolofScienceandEngineering. Wang,Chieh-Chih,CharlesThorpe,SebastianThrun,MartialHebert,andHughDurrant-Whyte.2007.Si-multaneousLocalization,MappingandMovingObjectTracking.TheInternationalJournalofRoboticsResearch26:889September. Wang,Xiang,andHongZhang.2007,April.AUPF-UKFFrameworkforSLAM.IEEEInternationalConferenceonRoboticsandAutomation.IEEE:IEEE,1664. Wolf,J.,W.Burgard,andH.Burkhardt.2005.Robustvision-basedlocalizationbycombininganimageretrievalsystemwithMonteCarlolocalization.IEEETransactionsonRoboticsandAutomation21:208. Zadeh,Lofti.1973.Outlineofanewapproachtotheanalysisofcomplexsystemsanddecisionprocesses.IEEETransactionsonSystems,ManandCybernetics1:28. Zhang,Liping,HuanjunYu,andShangxuHu.2003.ChapterANewApproachtoImproveParticleSwarmOptimizationofGeneticandEvolutionaryComputation-GECCO2003.Springer-VerlagBerlinHei-delberg. 88

PAGE 101

Zhang,P.,J.Gu,E.E.Milios,andP.Huynh.2005.NavigationwithIMU/GPS/digitalcompasswithun-scentedKalmanlter.2005IEEEInternationalConferenceMechatronicsandAutomation,Volume3.1497. 89

PAGE 102

Bibliography Arkin,R.C.,andR.R.Murphy.1990.Autonomousnavigationinamanufacturingenvironment.IEEETransactionsonRoboticsandAutomation6:445. Armesto,L.2007.Anasynchronousmulti-rateapproachtoprobablisticself-localizationandmapping.RoboticsandAutomationMagazine. Borges,G.A.,M.-J.Aldon,andT.Gil.2001.Anoptimalposeestimatorformap-basedmobilerobotdynamiclocalization:experimentalcomparisonwiththeEKF.Proceedingsofthe2001IEEEInterna-tionalConferenceonRoboticsandAutomationICRA,Volume2.1585. Borges,GeovanyAraujo,andMarie-JoseAldon.2002.OptimalMobileRobotPoseEstimationUsingGeometricalMaps.IEEETransactionsonRoboticsandAutomation18:87February. Bueckert,Jeff,SimonX.Yang,HongyinYuan,andMaxQ.-H.Meng.2007,August.KalmanFilterEnhancedTrackingControllerforMobileRobotswithBoundedAccelerations.Proceedingsofthe2007IEEEInternationalConferenceonMechatronicsandAutomation.IEEE,Harbin,China,17641770. Carpenter,J.,P.Clifford,andP.Fearnhead.1999.Improvedparticlelterfornonlinearproblems.IEEProceedings-Radar,SonarandNavigation146:2February. Chang,Chih-Fu,Ching-ChihTsai,Jui-ChengHsu,andChu-ChihLin.2003,June.LaserPoseTrackingforaMobileRobotUsingFuzzyAdaptiveExtendedInformationFiltering.ProceedingsoftheAmericanControlConference.IEEE,Denver,Colorado,2471. Chatterjee,P.S.,andT.L.Huntsberger.1988.Comparisonoftechniquesforsensorfusionunderun-certainconditions.ProceedingsofSPIESensorFusion:SpatialReasoningandSceneInterpretation1003:194. Cooper,S.,andHughDurrant-Whyte.1994.AKalmanFilterModelforGPSNavigationofLandVehicles.1994IEEEInternationalConferenceonIntelligentRobotsandSystems.157. Das,Tamoghna,andIndraNarayanKar.2006.DesignandImplementationofanAdaptiveFuzzyLogic-BasedControllerforWheeledMobileRobots.IEEETransactionsonControlSystemsTechnology14:501May. Dissanayake,M.W.M.Gamini,P.Newman,S.Clark,andHughDurrant-Whyte.2001.AsolutiontothesimultaneouslocalizationandmapbuildingSLAMproblem.IEEETransactionsonRoboticsandAutomation17:229June. Djouadi,MohandSaid,YacineMorsly,andDaoudBerkani.2007,June.AfuzzyIMM-UKFalgorithmforhighlymaneuveringmulti-targetvisual-basedtracking.MediterraneanConferenceonControlandAutomation2007.IEEE,Athens,Greece,1. Doucet,A.,NandodeFreitas,andN.Gordon.2001.SequentialMonteCarloMethodsinPractice.Heidel-berg:Springer. Doucet,A.,N.Gordon,andV.Krishnamurthy.2001a.ParticleFiltersforStateEstimationofJumpMarkovLinearSystems.IEEETransactionsonSignalProcessing49:613. Fox,D.,W.Burgard,andS.Thrun.1999.MarkovLocalizationforMobileRobotsinDynamicEnviron-ments.JournalofArticialIntelligenceResearch11:391. 90

PAGE 103

Frese,Udo,PerLarsson,andTomDuckett.2005.AMultilevelRelaxationAlgorithmforSimultaneousLocalizationandMapping.IEEETransactionsonRobotics21:196April. Grewal,M.S.,L.R.Weill,andA.P.Andrews.2001.GlobalPositioningSystems,InertialNavigation,andIntegration.Canada:JohnWileyandSons. Grisetti,Giorgio,SlawomirGrzonka,CyrillStachniss,PatrickPfaff,andWolframBurgard.2007,October.Efcientestimationofaccuratemaximumlikelihoodmapsin3D.IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems,IROS2007.3472. Gutmann,J.,W.Burgard,D.Fox,andK.Konolige.1998.Anexperimentalcomparisonoflocalizationmethods.Proc.oftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystemsIROS'98. Hiebert-Treuer,Bradley.2007.AnIntroductiontoRobotSLAMSimultaneousLocalizationAndMap-ping.Master'sthesis,MiddleburyCollege. Hong,L.,andG.-J.Wang.1994.Integratingmultisensornoisyandfuzzydata.Proceedingsofthe1994InternationalConferenceonMulti-SensorFusionandIntegrationforIntelligentSystems.LasVegas,NV,199. Huster,Andreas.2003.Relativepositionsensingbyfusingmonocularvisionandinertialratesensors.Ph.D.diss.,Stanford,CA,USA.Adviser-StephenM.Rock. Jekeli,C.2001.InertialNavigationSystemswithGeodeticApplications.EditedbyWalterdeGruyter.Canada:JohnWileyandSons. Kam,Moshe,XiaoxunZhu,andPaulKalata.1997.SensorFusionforMobileRobotNavigation.Pro-ceedingsoftheIEEE,Volume85.IEEE,108. Kamel,H.,andW.Badawy.2005.Fuzzylogicbasedparticlelterfortrackingamaneuverabletarget.48thMidwestSymposiumonCircuitsandSystems2August:1537. Kandel,Abraham,andGideonLangholz,eds.1998.FuzzyHardware:ArchitecturesandApplications.Springer. Kleeman,L.,andR.Kuc.1995.Mobilerobotsonarfortargetlocalizationandclassication.InternationalJournalofRoboticsResearch14:295. Kramer,Jeffrey,andAbrahamKandel.2009.FuzzyApproachestoDrivenKalmanFilteringforSmallRobotLocalization.InternationalConferenceonAdvancedRobotics.Munich. Lapin,B.D.1992.AdaptivePositionEstimationforanAutomatedGuidedVehicle.InMobileRobots7,Volume1831,82.SPIE. Lee,C.C.Mar/Apr1990a.Fuzzylogicincontrolsystems:fuzzylogiccontroller.I.Systems,ManandCybernetics,IEEETransactionson20:404. Luo,RenC.,andTseMinChen.2000.AutonomousMobileTargetTrackingSystemBasedonGrey-FuzzyControlAlgorithm.IEEETransactionsonIndustrialElectronics47:920August. Maeyama,S.,A.Ohya,andS.Yuta.1995a.Non-stopoutdoornavigationofamobilerobot.ProceedingsoftheInternationalConferenceonIntelligentRobotsandSystems,Volume1.130. .5-9Aug1995b.Non-stopoutdoornavigationofamobilerobot-retroactivepositioningdatafusionwithatimeconsumingsensorsystem.IntelligentRobotsandSystems95.'HumanRobotInteractionandCooperativeRobots',Proceedings.1995IEEE/RSJInternationalConferenceon1:130vol.1. McInroy,JohnE.2007,April.NearlyAnalyticalPoseEstimation.2007IEEEInternationalConferenceonRoboticsandAutomation.IEEE,Roma,Italy,4232. Mourikis,A.I.,andS.I.Roumeliotis.2006,May.Onthetreatmentofrelative-posemeasurementsformobilerobotlocalization.IEEEInternationalConferenceonRoboticsandAutomation.Orlando,FL,2277. 91

PAGE 104

Moutarlier,P.,andR.Chatlia.1989.Stochasticmultisensordatafusionformobilerobotlocalizationandenvironmentmodeling.Int.Symp.Robot.Res.,pp.85. Murphy,RobinR.1998.Dempster-ShaferTheoryforSensorFusioninAutonomousMobileRobots.IEEETransactionsonRoboticsandAutomation14:197April. Mutambara,A.G.O.1998.DecentralizedEstimationandControlforMultisensorSystems.CRCPress. Nuchter,Andreas,KaiLingemann,JoachinHertzberg,andHartmutSurmann.2007.DSLAM-3DMappingOutdoorEnvironments.JournalofFieldRobotics24/9:699. Ojeda,Lauro,andJohannBorenstein.2002,May.FLEXnav:FuzzyLogicExpertRule-basedPositionEstimationforMobileRobotsonRuggedTerrain.Proceedingsofthe2002IEEEInternationalCon-ferenceonRoboticsandAutomation.IEEE,Washington,DC,317. Ojeda,Lauro,MukundaRaju,andJohannBorenstein.2004,December.FLEXnav:AFuzzyLogicEx-pertDead-reckoningSystemfortheSegwayRMP.ProceedingsoftheSPIEDefenseandSecuritySymposium,VolumeOR54ofUnmannedGroundVehicleTechnologyVI.Orlando,FL. Olson,C.F.2000.Probablisticself-localizationformobilerobots.IEEETransactionsonRoboticsandAutomation16:55. Pacis,E.Biagtan,H.R.Everett,N.Farrington,andD.Bruemmer.2004.EnhancingFunctionalityandAutonomyinMan-PortableRobots.SPIEDefenseandSecuritySymposium2004. Peng,Jinzhu,YaonanWang,andHongshanYu.2007.NeuralNetwork-BasedRobustTrackingControlforNonholonomicMobileRobot.EditedbyD.Liu.ISNN2007. Piasecki,M.1994.Mobilerobotlocalizationbyfuzzylogicfusionofmultisensordata.RoboticsandAutonomousSystems12:155. Rehbinder,H.,andB.Ghosh.2001a.Multi-ratefusionofvisualandinertialdata.InternationalConfer-enceonMulti-SensorFusionandIntegrationforIntelligentSystems.97. Rehbinder,H.,andB.K.Ghosh.2001b.Multi-ratefusionofvisualandinertialdata.MultisensorFusionandIntegrationforIntelligentSystems,2001.MFI2001.InternationalConferenceon,pp.97. Simmons,R.G.,andS.Koenig.1995.Probabilisticrobotnavigationinpartiallyobservableenvornments.ProceedingsoftheInternationalJointConferenceonArticialIntelligenceIJCAI. Tanner,MartinA.,andWingHungWong.1987.TheCalculationofPosteriorDistributionsbyDataAugmentation:Rejoinder.JournaloftheAmericanStatisticalAssociation82:548. Tenne,D.,andT.Singh.2003.Thehigherorderunscentedlter.Proceedingsofthe2003AmericanControlConference3June:2441. Thrun,Sebastian,D.Fox,W.Burgard,andF.Dellaert.2001.RobustMonteCarlolocalizationformobilerobots.ArticialIntelligenceJournal128-2:99May. vanderMerwe,R.,andE.Wan.2003.Gaussianmixturesigma-pointparticleltersforsequentialprob-abilisticinferenceindynamicstate-spacemodels.ProceedingsoftheIEEEInternationalConferenceonAcoustics,SpeechandSignalProcessing6:VIVI. vanderMerwe,R.,andE.A.Wan.2001.Thesquare-rootunscentedKalmanlterforstateandparameter-estimation.Proceedingsofthe2001IEEEInternationalConferenceonAcoustics,Speech,andSignalProcessingICASSP'01,Volume6.3461. Vertechy,Rocco,andVincenzoParentiCastelli.2007.Accurateandfastbodyposeestimationbythreepointpositiondata.MechanismandMachineTheory42October:1170. Wan,E.,andR.V.derMerwe.2001.ChapterChapter7:TheUnscentedKalmanFilterofKalmanFilteringandNeuralNetworks.WileyPublishing. 92

PAGE 105

Wang,Chieh-Chih,C.Thorpe,andS.Thrun.14-19Sept.2003.Onlinesimultaneouslocalizationandmappingwithdetectionandtrackingofmovingobjects:theoryandresultsfromagroundvehicleincrowdedurbanareas.RoboticsandAutomation,2003.Proceedings.ICRA'03.IEEEInternationalConferenceon1:842vol.1. Welch,G.,andG.Bishop.2001.AnIntroductiontotheKalmanFilter.SIGGRAPH2001. Wijesoma,W.S.,L.D.L.Perera,andM.D.Adams.2006.Towardmultidimensionalassignmentdataasso-ciationinrobotlocalizationandmapping.IEEETransactionsonRobotics22:350April. 93

PAGE 106

Appendices 94

PAGE 107

AppendixAMathematicalFilterCode controllersControl.javapackagecontrollers;importjava.util.Vector;importJama.Matrix;publicinterfaceControlfvoidstartProcess;voidinputStateVector>State;voidinputStateMatrixState;BooleanfinishedProcess;g 95

PAGE 108

AppendixAContinued DoubleFuzzySPKF.javapackagecontrollers;importjava.util.Vector;importjavax.vecmath.GMatrix;importJama.Matrix;publicclassDoubleFuzzySPKFimplementsSPKFControlfpublicDoubleFuzzySPKFStringFNfgpublicDoublegetAlphaf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicDoublegetBetaf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicDoublegetGammaf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicDoublegetKappaf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;g 96

PAGE 109

AppendixAContinued publicDoublegetLambdaf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicMatrixgetPf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicvoidinputStateMatrixCov,MatrixInnof//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidsetAlphaDoubleLamf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidsetBetaDoubleBetf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidsetKappaDoubleKapf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidsetLintLf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicBooleanfinishedProcessf 97

PAGE 110

AppendixAContinued //TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicvoidinputStateVector>Statef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidinputStateMatrixStatef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidstartProcessf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicintgetControlTypef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturn0;gg 98

PAGE 111

AppendixAContinued EKFControl.javapackagecontrollers;importjava.util.Vector;//importjavax.vecmath.GMatrix;publicinterfaceEKFControlextendsControlfVectorgetQ;//ThreeDoubleVector)]TJ/F29 7.9701 Tf 11.4765 0 Td[(oneQforeachdirectionVectorgetR;//ThreeDoubleVector)]TJ/F29 7.9701 Tf 11.4765 0 Td[(oneRforeachdirectionVectorgetAlpha;//voidinputStateGMatrixState;//3x3!g 99

PAGE 112

AppendixAContinued FuzzyEKF.javapackagecontrollers;importjava.util.Vector;importJama.Matrix;//ImporttheFuzzyLogicsystemimportnet.sourceforge.jFuzzyLogic.FIS;importnet.sourceforge.jFuzzyLogic.rule.FuzzyRuleSet;publicclassFuzzyEKFimplementsEKFControlfprivateVectorvdAlpha=newVector;privateVectorvdQ=newVector;privateVectorvdR=newVector;privateVectorvdP=newVector;privateVectorvdMinus=newVector;privateVectorvdXYZ=newVector;privateBooleanStarted=false;privateBooleanDone=false;FISfis=null;FuzzyRuleSetfrs=null;publicFuzzyEKFStringFNfsuper;fis=FIS.loadFN;frs=fis.getFuzzyRuleSet;g 100

PAGE 113

AppendixAContinued publicVectorgetAlphafreturnvdAlpha;gpublicVectorgetQfreturnvdQ;gpublicVectorgetRfreturnvdR;gpublicBooleanfinishedProcessfifDonefDone=false;Started=false;return!Done;greturnDone;gpublicvoidinputStateVector>Statefif!StartedfvdXYZ=State.elementAt0;vdMinus=State.elementAt1;vdP=State.elementAt2;vdQ=State.elementAt3;vdR=State.elementAt4;vdAlpha=State.elementAt5;gelsefSystem.out.printlnCan't update the FuzzyEKF state while running!;ggpublicvoidstartProcessf 101

PAGE 114

AppendixAContinued if!StartedfStarted=true;DoubleX,Y,Z;DoubleXMinus,YMinus,ZMinus;DoublePX,PY,PZ;DoubledQx,dQy,dQz;DoubledRx,dRy,dRz;Doublealphax,alphay,alphaz;X=vdXYZ.elementAt0;Y=vdXYZ.elementAt1;Z=vdXYZ.elementAt2;XMinus=vdMinus.elementAt0;YMinus=vdMinus.elementAt1;ZMinus=vdMinus.elementAt2;PX=vdP.elementAt0;PY=vdP.elementAt1;PZ=vdP.elementAt2;dQx=vdQ.elementAt0;dQy=vdQ.elementAt1;dQz=vdQ.elementAt2;dRx=vdR.elementAt0;dRy=vdR.elementAt1;dRz=vdR.elementAt2;alphax=vdAlpha.elementAt0;alphay=vdAlpha.elementAt1;alphaz=vdAlpha.elementAt2;//Hopefullywedon'thavetobreakitapartbydirection...BUTWE//DO!//FuzzyWork//Xfrs.setVariablecovar,PX/dRx;frs.setVariableavg,XMinus)]TJ/F16 7.9701 Tf 10.281 0 Td[(X;frs.evaluate;alphax=frs.getVariablealpha.defuzzify; 102

PAGE 115

AppendixAContinued //Yfrs.setVariablecovar,PY/dRy;frs.setVariableavg,YMinus)]TJ/F16 7.9701 Tf 10.281 0 Td[(Y;frs.evaluate;alphay=frs.getVariablealpha.defuzzify;//Zfrs.setVariablecovar,PZ/dRz;frs.setVariableavg,ZMinus)]TJ/F16 7.9701 Tf 10.7233 0 Td[(Z;frs.evaluate;alphaz=frs.getVariablealpha.defuzzify;//UpdatealphaandR/QdQx=dQxalphax;//Math.powalphax,4;dRx=dRxalphax;//Math.powalphax,4;dQy=dQyalphay;//Math.powalphay,4;dRy=dRyalphay;//Math.powalphay,4;dQz=dQzalphaz;//Math.powalphaz,4;dRz=dRzalphaz;//Math.powalphaz,4;//SanityifdQx>100.0dQx=100.0;ifdQx<0.001dQx=0.001;ifdQy>100.0dQy=100.0;ifdQy<0.001dQy=0.001;ifdQz>100.0dQz=100.0;ifdQz<0.001dQz=0.001;ifdRx>100.0dRx=100.0;ifdRx<0.001dRx=0.001; 103

PAGE 116

AppendixAContinued ifdRy>100.0dRy=100.0;ifdRy<0.001dRy=0.001;ifdRz>100.0dRz=100.0;ifdRz<0.001dRz=0.001;//DEBUG//TODORemovethislater//System.out.printlndQx,dQy,dQz;//System.out.printlndQx+,+dQy+,+dQz;//System.out.printlndRx,dRy,dRz;//System.out.printlndRx+,+dRy+,+dRz;vdQ.clear;vdR.clear;vdAlpha.clear;vdQ.adddQx;vdQ.adddQy;vdQ.adddQz;vdR.adddRx;vdR.adddRy;vdR.adddRz;//vdAlpha.addalphax;//vdAlpha.addalphay;//vdAlpha.addalphaz;vdAlpha.add1.0;vdAlpha.add1.0;vdAlpha.add1.0;Done=true;gelsefSystem.out.printlnCan't start the FuzzyEKF while its running!;g 104

PAGE 117

AppendixAContinued gpublicvoidinputStateMatrixStatefif!Startedf//double[][]dState=State.getArray;vdXYZ.clear;vdMinus.clear;vdP.clear;vdQ.clear;vdR.clear;vdAlpha.clear;vdXYZ.addState.get0,0;vdXYZ.addState.get0,1;vdXYZ.addState.get0,2;vdMinus.addState.get1,0;vdMinus.addState.get1,1;vdMinus.addState.get1,2;vdP.addState.get2,0;vdP.addState.get2,1;vdP.addState.get2,2;vdQ.addState.get3,0;vdQ.addState.get3,1;vdQ.addState.get3,2;vdR.addState.get4,0;vdR.addState.get4,1;vdR.addState.get4,2;vdAlpha.addState.get5,0;vdAlpha.addState.get5,1;vdAlpha.addState.get5,2;gelsefSystem.out.printlnCan't update the IAEEKF state while running!;ggg 105

PAGE 118

AppendixAContinued FuzzySPKF.javapackagecontrollers;importjava.util.Vector;//ImporttheFuzzyLogicsystemimportnet.sourceforge.jFuzzyLogic.FIS;importnet.sourceforge.jFuzzyLogic.rule.FuzzyRuleSet;importJama.Matrix;/ThisistheFuzzyControllerfortheSPKF.Itincludesboththesingleanddoublefuzzycontrolmethods.@categoryController@authorjeffreykramer/publicclassFuzzySPKFimplementsSPKFControlfFISfisA=null;FISfisB=null;FISfisC=null;FuzzyRuleSetfrsA=null;FuzzyRuleSetfrsB=null;FuzzyRuleSetfrsC=null;privateMatrixP;privateMatrixInnov; 106

PAGE 119

AppendixAContinued privateMatrixDeviation;privateDoubleAlpha;privateDoubleBeta;privateDoubleKappa;privateintL;privateBooleanStarted=false;privateBooleanDone=false;privateBooleanDFK;privateBooleanbGPS;/SingleFuzzyConstructor@paramFN@paramiL/publicFuzzySPKFStringFN,intiLfsuper;fisA=FIS.loadFN;frsA=fisA.getFuzzyRuleSet;L=iL;DFK=false;bGPS=false;g/DoubleFuzzyConstructor 107

PAGE 120

AppendixAContinued @paramFNA@paramFNB@paramiL/publicFuzzySPKFStringFNA,StringFNB,intiLfsuper;fisA=FIS.loadFNA;frsA=fisA.getFuzzyRuleSet;fisB=FIS.loadFNB;frsB=fisB.getFuzzyRuleSet;fisC=FIS.loadnewdfkfis.txt;frsC=fisC.getFuzzyRuleSet;L=iL;DFK=true;bGPS=false;gpublicMatrixgetPf//ReturntheMatrixP...butnotunlesswe'redonewithit!ifDone&&StartedfDone=false;Started=false;returnP;gelsefSystem.out.printlnNot done with P! You can't have it!;greturnnull;gpublicBooleanfinishedProcessfreturnDone;gpublicvoidinputStateVector>Statef 108

PAGE 121

AppendixAContinued System.out.printlnIn the interests of time, we don't support vector vector madness here. Try again later.;gpublicvoidinputStateMatrixStatefSystem.out.printlnWe don't do this here... =0;gpublicvoidinputStateMatrixCov,MatrixInnof//'erewego!P=Cov;Innov=Inno;gpublicvoidisGPSMatrixoIMUfbGPS=true;Deviation=oIMU;gpublicvoidstartProcessfif!StartedfStarted=true;//PhasPxandourCovariancesinit//InnovisourinnovationsDoubleInnoX=Innov.get0,0;DoubleInnoY=Innov.get1,0;DoubleInnoZ=Innov.get2,0;DoublePX=P.get0,0;DoublePY=P.get1,1;DoublePZ=P.get2,2;DoubledQx=P.get3,3;DoubledQy=P.get4,4;DoubledQz=P.get5,5;DoubledRx=P.get6,6;DoubledRy=P.get7,7;DoubledRz=P.get8,8; 109

PAGE 122

AppendixAContinued Doublealphax,alphay,alphaz;//System.out.printlnInFuzzySPKF!;ifDFKf//System.out.printlnInDoubleFuzzySPKF!;//TWOMETHODS://1)]TJ/F29 7.9701 Tf 11.5071 0 Td[(GreekModification//2)]TJ/F29 7.9701 Tf 11.7031 0 Td[(Fusionvs.IMU)]TJ/F29 7.9701 Tf 11.6978 0 Td[(persteperrorvs.sensorresponseif!bGPSf//System.out.printlnInOldDouble;//So,inherewe'llbeeditingourgreeks//alphamust0<=a<=1,withtendingtowardszero//betamustbe>0with2representingaGaussiansystem//kappamustbegreaterthanzero,withnocapunknown//providenceDoublefalpha,fbeta,fkappa;//Thisdoesn'tworkonindividualcovariances,sowe'll//average//ourcovariance,andourinnovationswhichshouldwork,//but//mustbetested//TODOMakesurethataveragingtheseworksappropriatelyDoubleInnoAvg=InnoX+InnoY+InnoZ/3;DoublePAvg=PX+PY+PZ/3;DoubleRAvg=dRx+dRy+dRz/3;InnoAvg=Math.absInnoAvg;//System.out.printlnavg:+InnoAvg;//System.out.printlncovar:+PAvg/RAvg; 110

PAGE 123

AppendixAContinued frsB.setVariablecovar,Math.absPAvg/RAvg;frsB.setVariableavg,InnoAvg;frsB.evaluate;falpha=frsB.getVariablealphamod.defuzzify;fbeta=frsB.getVariablebetamod.defuzzify;//fkappa=frsB.getVariablekappamod.defuzzify;//System.out.printlnFuzzyMults:+falpha+,+//fbeta;//CheckforSanity!falpha=falphaAlpha;iffalpha>0.4falpha=0.4;iffalpha<0.000001falpha=0.000001;fbeta=fbetaBeta;iffbeta>4fbeta=4.0;iffbeta<=0.5fbeta=0.5;this.setAlphafalpha;this.setBetafbeta;//this.setKappafkappaKappa;gelsef//ThisisthenewmethodofDFSPKF)]TJ/F29 7.9701 Tf 11.8707 0 Td[(checkingpersteperrorofIMUvs.SensorResponse//System.out.printlnInNewDFSPKF;doubleTrueDev=Math.sqrtDeviation.get0,0Deviation.get0,0+Deviation.get1,0Deviation.get1,0;//System.out.printlnTrueDev=+TrueDev;frsC.setVariabledev,TrueDev;frsC.evaluate;doublegmod=frsC.getVariablegmod.defuzzify; 111

PAGE 124

AppendixAContinued //System.out.printlngmod:+gmod;dRx=dRxgmod;dRy=dRygmod;dRz=dRzgmod;//System.out.printlndR=+dRx+,+dRy+,+dRz;gg//FuzzyWork//XfrsA.setVariablecovar,PX/dRx;frsA.setVariableavg,InnoX;frsA.evaluate;alphax=frsA.getVariablealpha.defuzzify;//YfrsA.setVariablecovar,PY/dRy;frsA.setVariableavg,InnoY;frsA.evaluate;alphay=frsA.getVariablealpha.defuzzify;//ZfrsA.setVariablecovar,PZ/dRz;frsA.setVariableavg,InnoZ;frsA.evaluate;alphaz=frsA.getVariablealpha.defuzzify;//System.out.printlndQ,dRMults:+alphax+,+alphay+,//+alphaz;//UpdateR/QdQx=dQxalphax;dQy=dQyalphay;dQz=dQzalphaz;if!bGPSjj!DFKfdRx=dRxalphax;dRy=dRyalphay;dRz=dRzalphaz; 112

PAGE 125

AppendixAContinued g//System.out.printlndQ:+dQx+,+dQy+,+dQz;//System.out.printlndR:+dRx+,+dRy+,+dRz;//SanityifdQx>100.0dQx=100.0;ifdQx<0.001dQx=0.001;ifdQy>100.0dQy=100.0;ifdQy<0.001dQy=0.001;ifdQz>100.0dQz=100.0;ifdQz<0.001dQz=0.001;ifdRx>100.0dRx=100.0;ifdRx<0.001dRx=0.001;ifdRy>100.0dRy=100.0;ifdRy<0.001dRy=0.001;ifdRz>100.0dRz=100.0;ifdRz<0.001dRz=0.001;P.set3,3,dQx;P.set4,4,dQy;P.set5,5,dQz;P.set6,6,dRx;P.set7,7,dRy; 113

PAGE 126

AppendixAContinued P.set8,8,dRz;Done=true;bGPS=false;//Started=false;ggpublicDoublegetAlphafreturnAlpha;gpublicDoublegetBetafreturnBeta;gpublicDoublegetGammafreturnMath.sqrtL+this.getLambda;gpublicDoublegetKappafreturnKappa;gpublicDoublegetLambdafreturnAlphaAlphaL+Kappa)]TJ/F16 7.9701 Tf 10.7234 0 Td[(L;gpublicvoidsetLintiLfSystem.out.printlnThis is likely a bad idea... You're changing L!;L=iL;gpublicvoidsetAlphaDoubleAlpfAlpha=Alp; 114

PAGE 127

AppendixAContinued gpublicvoidsetBetaDoubleBetfBeta=Bet;gpublicvoidsetKappaDoubleKapfKappa=Kap;gpublicintgetControlTypef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturn2;gg 115

PAGE 128

AppendixAContinued NoControlEKF.javapackagecontrollers;importjava.util.Vector;importJama.Matrix;publicclassNoControlEKFimplementsEKFControlfprivateBooleanStarted=false;privateBooleanDone=false;privateVectorvdAlpha=newVector;privateVectorvdQ=newVector;privateVectorvdR=newVector;//privateVectorvdP;//privateVectorvdMinus;//privateVectorvdXYZ;publicNoControlEKFfsuper;gpublicVectorgetAlphafreturnvdAlpha;gpublicVectorgetQfreturnvdQ;gpublicVectorgetRf 116

PAGE 129

AppendixAContinued returnvdR;gpublicBooleanfinishedProcessfifDonefDone=false;Started=false;return!Done;greturnDone;gpublicvoidinputStateVector>Statefif!Startedf//vdXYZ=State.elementAt0;//vdMinus=State.elementAt1;//vdP=State.elementAt2;vdQ=State.elementAt3;vdR=State.elementAt4;vdAlpha=State.elementAt5;gelsefSystem.out.printlnCan't update the NoControlEKF state while running!;ggpublicvoidstartProcessf//Notmuchinhere!if!StartedfStarted=true;//EMPTYDone=true;gelsefSystem.out.printlnCan't update the NoControlEKF state while running!;g 117

PAGE 130

AppendixAContinued gpublicvoidinputStateMatrixStatefif!Startedf//double[][]dState=State.getArray;//State.print3,2;vdQ.clear;vdR.clear;vdAlpha.clear;vdQ.addState.get3,0;vdQ.addState.get3,1;vdQ.addState.get3,2;vdR.addState.get4,0;vdR.addState.get4,1;vdR.addState.get4,2;vdAlpha.addState.get5,0;vdAlpha.addState.get5,1;vdAlpha.addState.get5,2;gelsefSystem.out.printlnCan't update the NoControlEKF state while running!;ggg 118

PAGE 131

AppendixAContinued NoControlPF.javapackagecontrollers;importjava.util.Vector;importJama.Matrix;publicclassNoControlPFimplementsPFControlfpublicBooleanfinishedProcessf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gpublicvoidinputStateVector>Statef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidinputStateMatrixStatef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgpublicvoidstartProcessf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubgg 119

PAGE 132

AppendixAContinued NoControlSPKF.javapackagecontrollers;importjava.util.Vector;importJama.Matrix;publicclassNoControlSPKFimplementsSPKFControlfprivateMatrixP;privateMatrixInnov;privateDoubleAlpha;privateDoubleBeta;privateDoubleKappa;privateintL;privateBooleanStarted=false;privateBooleanDone=false;publicNoControlSPKFintiLfL=iL;gpublicMatrixgetPf//ReturntheMatrixP...butnotunlesswe'redonewithit!ifDonefDone=false;returnP;gelsef 120

PAGE 133

AppendixAContinued System.out.printlnNot done with P! You can't have it!;greturnnull;gpublicBooleanfinishedProcessfreturnDone;gpublicvoidinputStateVector>StatefSystem.out.printlnIn the interests of time, we don't support vector vector madness here. Try again later.;gpublicvoidinputStateMatrixStatefSystem.out.printlnWe don't do this here... =0;gpublicvoidinputStateMatrixCov,MatrixInnof//'erewego!P=Cov;Innov=Inno;gpublicvoidstartProcessfStarted=true;//WewouldscrewwiththeQandRinhere.Done=true;Started=false;gpublicDoublegetAlphafreturnAlpha;g 121

PAGE 134

AppendixAContinued publicDoublegetBetafreturnBeta;gpublicDoublegetGammafreturnMath.sqrtL+this.getLambda;gpublicDoublegetKappafreturnKappa;gpublicDoublegetLambdafreturnAlphaAlphaL+Kappa)]TJ/F16 7.9701 Tf 10.7233 0 Td[(L;gpublicvoidsetLintiLfSystem.out.printlnThis is likely a bad idea... You're changing L!;L=iL;gpublicvoidsetAlphaDoubleAlpfAlpha=Alp;gpublicvoidsetBetaDoubleBetfBeta=Bet;gpublicvoidsetKappaDoubleKapfKappa=Kap;gpublicintgetControlTypef//ControllerType1 122

PAGE 135

AppendixAContinued return1;gg 123

PAGE 136

AppendixAContinued PFControl.javapackagecontrollers;publicinterfacePFControlextendsControlfg 124

PAGE 137

AppendixAContinued SPKFControl.javapackagecontrollers;importjava.util.Vector;//importjavax.vecmath.GMatrix;importJama.Matrix;publicinterfaceSPKFControlextendsControlfMatrixgetP;DoublegetAlpha;DoublegetGamma;DoublegetKappa;DoublegetLambda;DoublegetBeta;voidsetLintL;voidsetAlphaDoubleLam;voidsetKappaDoubleKap;voidsetBetaDoubleBet;voidinputStateMatrixCov,MatrixInno;intgetControlType;g ltersEKF.javapackagefilters;importjava.util.Date;importjava.util.Vector;importtestsystem.CompassEntry;importtestsystem.Data;importtestsystem.GPSEntry;importtestsystem.IMUEntry; 125

PAGE 138

AppendixAContinued importtestsystem.OdoEntry;importJama.Matrix;importcontrollers.;importjava.io.;publicclassEKFimplementsFilterf/@paramargs/privatebooleanProcessOn=false;privatebooleanProcessDone=false;privateDateTimeOn,TimeOff;privateLongTimeElapsed;privateDatadDataset;privateEKFControlcController;privateVectorOutput=newVector;privateVectorResid=newVector;privateDoubleinitQ,initR;publicEKFControlcontroller,DataDataset,DoubledefQ,DoubledefRfcController=EKFControlcontroller;dDataset=Dataset;initQ=defQ;initR=defR;gpublicBooleanfinishedProcessf 126

PAGE 139

AppendixAContinued returnProcessDone;gpublicvoidoutputDataStringFileNameftryfBufferedWriterout=newBufferedWriternewFileWriterFileName+TimeElapsed+.txt;intlen=Output.size;System.out.printlnTime Elapsed: +TimeElapsed;GPSEntryA;MatrixB;out.write#Timestep NorthX EastY ResidX ResidY ResidZ;out.newLine;forinti=0;i
PAGE 140

AppendixAContinued publicvoidsetDataDataDatasetfif!ProcessOnf//ChangetheDatasetdDataset=Dataset;gelseSystem.out.printlnCan't reset data in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidstartProcessf//Starttheprocess!ProcessOn=true;TimeOn=newDate;//GetthelengthofthedatasetintiLength=dDataset.getLength;GPSEntrygGPS;IMUEntryiIMU;OdoEntryoOdo;CompassEntrycComp;DoubledTS;VectorvIMU=newVector;VectorvComp=newVector;VectorvOdoR=newVector;VectorvOdo=newVector;VectorvGPS=newVector;VectorvOldDS=newVector;//BuildtheFirstStates!Vector>ZeroHeadingState=newVector>;Vector>HeadingState=newVector>;Vector>DriveState=newVector>;Vector>PoseState=newVector>;VectorTempVec=newVector;//X,Y,Z//XMinus,YMinus,ZMinus 128

PAGE 141

AppendixAContinued TempVec.add0.0;TempVec.add0.0;TempVec.add0.0;DriveState.addTempVec;DriveState.addTempVec;TempVec.set2,dDataset.GetCompassEntry0.GetRad;HeadingState.addTempVec;HeadingState.addTempVec;TempVec.set2,dDataset.GetOdoEntry0.getOdoTheta;ZeroHeadingState.addTempVec;ZeroHeadingState.addTempVec;TempVec.clear;TempVec.adddDataset.GetGPSEntry0.getEastX;TempVec.adddDataset.GetGPSEntry0.getNorthY;TempVec.add0.0;PoseState.addTempVec;PoseState.addTempVec;//PX,PY,PZTempVec.clear;TempVec.add0.0;TempVec.add0.0;TempVec.add0.0;ZeroHeadingState.addTempVec;HeadingState.addTempVec;DriveState.addTempVec;PoseState.addTempVec;//Qx,Qy,QzTempVec.clear;TempVec.addinitQ;TempVec.addinitQ;TempVec.addinitQ;ZeroHeadingState.addTempVec;HeadingState.addTempVec;DriveState.addTempVec;PoseState.addTempVec;//Rx,Ry,Rz 129

PAGE 142

AppendixAContinued TempVec.clear;TempVec.addinitR;TempVec.addinitR;TempVec.addinitR;ZeroHeadingState.addTempVec;HeadingState.addTempVec;DriveState.addTempVec;//Rx,Ry,RzTempVec.clear;TempVec.add2.5;TempVec.add2.5;TempVec.add2.5;PoseState.addTempVec;//Ax,Ay,AzTempVec.clear;TempVec.add1.0;TempVec.add1.0;TempVec.add1.0;ZeroHeadingState.addTempVec;HeadingState.addTempVec;DriveState.addTempVec;PoseState.addTempVec;DoubleOldOdoTheta=0.0;DoubleOldOdoX=0.0;DoubleOldOdoY=0.0;//Starttheroll!forinti=0;i
PAGE 143

AppendixAContinued //Tickerifi%1000==0System.out.print.;//CascadingEKFs!//KalmanFilteringSection//Step1:Cutaholeinabox//Step2:Putthejunkinthebox//Step3:Getthemtoopenthebox//Actually://Step1:KFODOThetaasthedriveandCompassastheMeasure//Step2:UsethecombinedheadingtolineralizetheIMUacceleration//Step3:KFlinearlizedaccelasadriveandODOXandYastheMeasure//Step4:KFCombinedasadriveandGPSXandYastheMeasure//Step0:KFtheproperorderagain...vIMU.clear;vIMU.add0.0;vIMU.add0.0;vIMU.addiIMU.getIMURotAccel.get2;vOdoR.clear;vOdoR.add0.0;vOdoR.add0.0;vOdoR.addoOdo.getOdoTheta)]TJ/F16 7.9701 Tf 6.7164 0 Td[(OldOdoTheta;OldOdoTheta=oOdo.getOdoTheta;ZeroHeadingState=KStepZeroHeadingState,vOdoR,vIMU,dTS;//Step1:vOdoR.clear;vOdoR=ZeroHeadingState.get0;vOdoR.set0,vOdoR.get0/dTS;vOdoR.set1,vOdoR.get1/dTS;vOdoR.set2,vOdoR.get2/dTS;vComp.clear;vComp.add0.0;vComp.add0.0; 131

PAGE 144

AppendixAContinued vComp.addcComp.GetRad;HeadingState=KStepHeadingState,vComp,vOdoR,dTS;//Step2:vIMU=dDataset.GetIMUVelocityi,HeadingState.get0;//Step3:vOdo.clear;vOdo.addoOdo.getOdoX)]TJ/F16 7.9701 Tf 6.2402 0 Td[(OldOdoX;vOdo.addoOdo.getOdoY)]TJ/F16 7.9701 Tf 6.2402 0 Td[(OldOdoY;vOdo.add0.0;OldOdoX=oOdo.getOdoX;OldOdoY=oOdo.getOdoY;DriveState=KStepDriveState,vOdo,vIMU,dTS;//Step4:vOdo.clear;vOdo.addDriveState.get0.get0/dTS;vOdo.addDriveState.get0.get1/dTS;vOdo.addDriveState.get0.get2/dTS;vGPS.clear;vGPS.addgGPS.getEastX;vGPS.addgGPS.getNorthY;vGPS.add0.0;PoseState=KStepPoseState,vGPS,vOdo,dTS;Output.addnewGPSEntryPoseState.elementAt0.elementAt0,PoseState.elementAt0.elementAt1,dTS;MatrixtempResid=newMatrix1,3,0.0;tempResid.set0,0,PoseState.get0.get0)]TJ/F16 7.9701 Tf 7.1894 0 Td[(PoseState.get1.get0;tempResid.set0,1,PoseState.get0.get1)]TJ/F16 7.9701 Tf 7.1894 0 Td[(PoseState.get1.get1;tempResid.set0,2,PoseState.get0.get2)]TJ/F16 7.9701 Tf 7.1894 0 Td[(PoseState.get1.get2;Resid.addtempResid;g//Processdone!TimeOff=newDate;TimeElapsed=TimeOff.getTime)]TJ/F16 7.9701 Tf 11.0712 0 Td[(TimeOn.getTime;ProcessOn=false; 132

PAGE 145

AppendixAContinued ProcessDone=true;gprivateVector>KStepVector>vOldState,VectordMeasurement,VectordStateUpdate,DoubledTimeStepf//BigKalman)]TJ/F29 7.9701 Tf 12.1758 0 Td[(individualdRanddQVector>vOutput=newVector>;//ExtractallofthenecessaryvariablesDoubleXKMinus=vOldState.elementAt0.elementAt0;DoubleYKMinus=vOldState.elementAt0.elementAt1;DoubleZKMinus=vOldState.elementAt0.elementAt2;DoubleXPkminus=vOldState.elementAt2.elementAt0;DoubleYPkminus=vOldState.elementAt2.elementAt1;DoubleZPkminus=vOldState.elementAt2.elementAt2;DoubledQx=vOldState.elementAt3.elementAt0;DoubledQy=vOldState.elementAt3.elementAt1;DoubledQz=vOldState.elementAt3.elementAt2;DoubledRx=vOldState.elementAt4.elementAt0;DoubledRy=vOldState.elementAt4.elementAt1;DoubledRz=vOldState.elementAt4.elementAt2;Doublealphax=vOldState.elementAt5.elementAt0;Doublealphay=vOldState.elementAt5.elementAt1;Doublealphaz=vOldState.elementAt5.elementAt2;//ProjecttheNewStateaheadDoubleXMinus=XKMinus+dStateUpdate.elementAt0dTimeStep;DoubleYMinus=YKMinus+dStateUpdate.elementAt1dTimeStep;DoubleZMinus=ZKMinus+dStateUpdate.elementAt2dTimeStep;//ProjecttheErrorCovarianceAheadDoubleXPMinus=alphaxalphaxXPkminusalphaxalphax+dQx;DoubleYPMinus=alphayalphayYPkminusalphayalphay+dQy;DoubleZPMinus=alphazalphazZPkminusalphazalphaz+dQz; 133

PAGE 146

AppendixAContinued //ComputeKalmanGainDoubleXKal=XPMinus/XPMinus+dRx/alphaxalphax;DoubleYKal=YPMinus/YPMinus+dRy/alphayalphay;DoubleZKal=ZPMinus/ZPMinus+dRz/alphazalphaz;//UpdateEstimatewithmeasurementDoubleX=XMinus+XKaldMeasurement.elementAt0)]TJ/F16 7.9701 Tf 11.1338 0 Td[(XMinus;DoubleY=YMinus+YKaldMeasurement.elementAt1)]TJ/F16 7.9701 Tf 11.1339 0 Td[(YMinus;DoubleZ=ZMinus+ZKaldMeasurement.elementAt2)]TJ/F16 7.9701 Tf 11.2603 0 Td[(ZMinus;//UpdatetheErrorCovarianceDoublePX=1)]TJ/F16 7.9701 Tf 11.1403 0 Td[(XKal/XPMinus;DoublePY=1)]TJ/F16 7.9701 Tf 11.1403 0 Td[(YKal/YPMinus;DoublePZ=1)]TJ/F16 7.9701 Tf 11.3172 0 Td[(ZKal/ZPMinus;//ErrorCheckingPrintsforDebugifX.isNaNfSystem.out.printlnX is Not A Number! Freakout Time! Output: +X+,+Y+,+Z;System.out.printn007;System.out.printn007;System.out.printn007;System.out.printn007;System.out.flush;tryfThread.sleep10000;gcatchInterruptedExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;ggifY.isNaNfSystem.out.printlnY is Not A Number! Freakout Time! Output: +X+,+Y+,+Z;System.out.printn007;System.out.printn007; 134

PAGE 147

AppendixAContinued System.out.printn007;System.out.printn007;System.out.flush;tryfThread.sleep10000;gcatchInterruptedExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;ggifZ.isNaNfSystem.out.printlnZ is Not A Number! Freakout Time! Output: +X+,+Y+,+Z;System.out.printlnMinus +XMinus+,+YMinus+,+ZMinus;System.out.printlnPMinus +XPMinus+,+YPMinus+,+ZPMinus;System.out.printlnKal +XKal+,+YKal+,+ZKal;System.out.printlnP +PX+,+PY+,+PZ;System.out.printlnQ +dQx+,+dQy+,+dQz;System.out.printlnR +dRx+,+dRy+,+dRz;System.out.printn007;System.out.printn007;System.out.printn007;System.out.printn007;System.out.flush;tryfThread.sleep10000;gcatchInterruptedExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;gg//OutputVectorRowZero=newVector;RowZero.addX;RowZero.addY; 135

PAGE 148

AppendixAContinued RowZero.addZ;//NOTOUTPUT...=0VectorRowOne=newVector;RowOne.addXMinus;RowOne.addYMinus;RowOne.addZMinus;VectorRowTwo=newVector;RowTwo.addPX;RowTwo.addPY;RowTwo.addPZ;vOutput.addRowZero;vOutput.addRowOne;vOutput.addRowTwo;vOutput.addvOldState.elementAt3;vOutput.addvOldState.elementAt4;vOutput.addvOldState.elementAt5;//UpdatealphaandR/QcController.inputStatevOutput;cController.startProcess;while!cController.finishedProcessftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggvOutput.remove3;vOutput.remove3;vOutput.remove3;vOutput.addcController.getQ;vOutput.addcController.getR;vOutput.addcController.getAlpha; 136

PAGE 149

AppendixAContinued returnvOutput;gpubliclonggetTimeElapsedfreturnTimeElapsed;gpublicVectorgetOutputfifProcessDonefreturnOutput;greturnnull;gpublicVectorgetResidualsfifProcessDonefreturnResid;greturnnull;gg 137

PAGE 150

AppendixAContinued Filter.javapackagefilters;importjava.util.Vector;importJama.Matrix;importtestsystem.Data;importtestsystem.GPSEntry;importcontrollers.Control;publicinterfaceFilterfvoidsetDataDataDataset;BooleanfinishedProcess;voidstartProcess;voidsetControlControlController;voidoutputDataStringFileName;longgetTimeElapsed;VectorgetOutput;VectorgetResiduals;g 138

PAGE 151

AppendixAContinued MHEKF.javapackagefilters;importjava.io.BufferedWriter;importjava.io.FileWriter;importjava.io.IOException;importjava.util.Calendar;importjava.util.Date;importjava.util.Vector;importJama.Matrix;importtestsystem.Data;importtestsystem.GPSEntry;importcontrollers.Control;importcontrollers.EKFControl;publicclassMHEKFimplementsFilterfprivateintNumFilters=0;privateVectorFilterList=newVector;privateVector>Outputs=newVector>;privateVector>Resids=newVector>;privateVectorElapsedTimes=newVector;privatebooleanProcessOn=false;privatebooleanProcessDone=false;privateDateTimeOn,TimeOff;privateLongTimeElapsed; 139

PAGE 152

AppendixAContinued privateDatadDataset;DoubledefQ,defR;VectorOutput=newVector;VectorResid=newVector;publicMHEKFControlController,DataDataset,DoubleinitQ,DoubleinitRfdefQ=initQ;defR=initR;FilterList.addnewEKFEKFControlController,Dataset,defQ,defR;NumFilters++;dDataset=Dataset;gpublicBooleanfinishedProcessfreturnProcessDone;gpublicvoidoutputDataStringFileNamefintiLen=FilterList.size;forintj=0;j
PAGE 153

AppendixAContinued out.writeA.getGPSTimestamp+ +A.getEastX+ +A.getNorthY+ +B.get0,0+ +B.get0,1+ +B.get0,2;out.newLine;gout.close;gcatchIOExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;ggpublicvoidsetControlControlControllerf//Thisdiffersinimplementationfromtheotherfilters)]TJ/F29 7.9701 Tf 12.4782 0 Td[(itadds//multiplecontrollerstothesamefiltertype,untiltheystopadding//them.if!ProcessOnfFilterList.addnewEKFController,dDataset,defQ,defR;NumFilters++;gelsefSystem.out.printlnCan't add controllers to the MHEKF while its running!;ggpublicvoidsetControlControlController,DoubleinitQ,DoubleinitRf//Thisdiffersinimplementationfromtheotherfilters)]TJ/F29 7.9701 Tf 12.4782 0 Td[(itadds//multiplecontrollerstothesamefiltertype,untiltheystopadding//them.if!ProcessOnfFilterList.addnewEKFController,dDataset,initQ,initR;NumFilters++;gelsefSystem.out.printlnCan't add controllers to the MHEKF while its running!;gg 141

PAGE 154

AppendixAContinued publicvoidsetDataDataDatasetfdDataset=Dataset;gpublicvoidstartProcessf//Starttheprocess!ProcessOn=true;TimeOn=newDate;//GetthelengthofthedatasetintiLength=dDataset.getLength;Filtertemp;forinti=0;iWeights=newVector;MatrixTotalWeight=newMatrix1,3,0.0;forintj=0;j
PAGE 155

AppendixAContinued //System.out.printlnResid+j+at+i;//Resids.getj.geti.print3,2;TotalResid=TotalResid.plusResids.getj.geti;g//System.out.printlnTotalResid;//System.out.printlnTotalResid.get0,0+,+TotalResid.get0,1+,+TotalResid.get0,2;forintj=0;j1)]TJ/F29 7.9701 Tf 6.5712 0 Td[(Rn/RtMatrixtempWeight=newMatrix1,3,0.0;MatrixtempI=newMatrix1,3,1.0;//tempWeight=Resids.getj.geti.timesTotalResid.inverse;tempWeight.set0,0,Resids.getj.geti.get0,0/TotalResid.get0,0;tempWeight.set0,1,Resids.getj.geti.get0,1/TotalResid.get0,1;tempWeight.set0,2,Resids.getj.geti.get0,2/TotalResid.get0,2;tempWeight=tempI.minustempWeight;TotalWeight=TotalWeight.plustempWeight;Weights.addtempWeight;//System.out.println1)]TJ/F29 7.9701 Tf 7.0683 0 Td[(Rn/Rt;//tempWeight.print3,2;g//System.out.printlnTotalWeight;//TotalWeight.print3,2;MatrixChi=newMatrix1,3,0.0;forintj=0;j
PAGE 156

AppendixAContinued Output.addnewGPSEntryChi.get0,0/tempF.get0,0,Chi.get0,1/tempF.get0,1,Outputs.get0.geti.getGPSTimestamp;MatrixtempR=newMatrix1,3,0.0;GPSEntrygGPS=dDataset.GetGPSEntryi;tempR.set0,0,gGPS.getEastX)]TJ/F16 7.9701 Tf 6.6925 0 Td[(Chi.get0,0/tempF.get0,0;tempR.set0,1,gGPS.getNorthY)]TJ/F16 7.9701 Tf 6.6925 0 Td[(Chi.get0,1/tempF.get0,1;Resid.addtempR;g//Processdone!TimeOff=newDate;TimeElapsed=TimeOff.getTime)]TJ/F16 7.9701 Tf 11.0712 0 Td[(TimeOn.getTime;ProcessOn=false;ProcessDone=true;gpubliclonggetTimeElapsedf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturn0;gpublicVectorgetOutputfifProcessDonefreturnOutput;greturnnull;gpublicVectorgetResidualsf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;g 144

PAGE 157

AppendixAContinued g 145

PAGE 158

AppendixAContinued MHSPKF.javapackagefilters;importjava.io.BufferedWriter;importjava.io.FileWriter;importjava.io.IOException;importjava.util.Date;importjava.util.Vector;importJama.Matrix;importtestsystem.Data;importtestsystem.GPSEntry;importcontrollers.Control;importcontrollers.SPKFControl;publicclassMHSPKFimplementsFilterfprivateintNumFilters=0;privateVectorFilterList=newVector;privateVector>Outputs=newVector>;privateVector>Resids=newVector>;VectorResid=newVector;privateVectorElapsedTimes=newVector;privatebooleanProcessOn=false;privatebooleanProcessDone=false;privateDateTimeOn,TimeOff; 146

PAGE 159

AppendixAContinued privateLongTimeElapsed;privateDatadDataset;privateDoubledQ;privateDoubledR;privateDoubleAlpha,Beta,Kappa;VectorOutput=newVector;publicMHSPKFControlController,DataDataset,DoubleinitQ,DoubleinitRfFilterList.addnewSPKFSPKFControlController,Dataset,initQ,initR;NumFilters++;dDataset=Dataset;dQ=initQ;dR=initR;Alpha=0.02;Beta=0.2;Kappa=0.0;gpublicMHSPKFControlController,DataDataset,DoubleinitQ,DoubleinitR,DoubleiAlpha,DoubleiBeta,DoubleiKappafFilterList.addnewSPKFSPKFControlController,Dataset,initQ,initR,iAlpha,iBeta,iKappa;NumFilters++;dDataset=Dataset;dQ=initQ;dR=initR;Alpha=iAlpha;Beta=iBeta;Kappa=iKappa;g 147

PAGE 160

AppendixAContinued publicBooleanfinishedProcessfreturnProcessDone;gpublicvoidoutputDataStringFileNamefintiLen=FilterList.size;forintj=0;j
PAGE 161

AppendixAContinued FilterList.addnewSPKFSPKFControlController,dDataset,dQ,dR;NumFilters++;gelsefSystem.out.printlnCan't add controllers to the MHSPKF while its running!;ggpublicvoidsetControlControlController,DoubleinitQ,DoubleinitRf//Thisdiffersinimplementationfromtheotherfilters)]TJ/F29 7.9701 Tf 12.4782 0 Td[(itadds//multiplecontrollerstothesamefiltertype,untiltheystopadding//them.if!ProcessOnfFilterList.addnewSPKFSPKFControlController,dDataset,initQ,initR;NumFilters++;gelsefSystem.out.printlnCan't add controllers to the MHSPKF while its running!;ggpublicvoidsetControlControlController,DoubleinitQ,DoubleinitR,DoubleiAlpha,DoubleiBeta,DoubleiKappaf//Thisdiffersinimplementationfromtheotherfilters)]TJ/F29 7.9701 Tf 12.4782 0 Td[(itadds//multiplecontrollerstothesamefiltertype,untiltheystopadding//them.if!ProcessOnfFilterList.addnewSPKFSPKFControlController,dDataset,initQ,initR,iAlpha,iBeta,iKappa;NumFilters++;gelsefSystem.out.printlnCan't add controllers to the MHSPKF while its running!;gg 149

PAGE 162

AppendixAContinued publicvoidsetDataDataDatasetfdDataset=Dataset;gpublicvoidstartProcessf//Starttheprocess!ProcessOn=true;TimeOn=newDate;//GetthelengthofthedatasetintiLength=dDataset.getLength;Filtertemp;forinti=0;iWeights=newVector;MatrixTotalWeight=newMatrix1,3,0.0;forintj=0;j
PAGE 163

AppendixAContinued //System.out.printlnResid+j+at+i;//Resids.getj.geti.print3,2;TotalResid=TotalResid.plusResids.getj.geti;g//System.out.printlnTotalResid;//System.out.printlnTotalResid.get0,0+,+TotalResid.get0,1+,+TotalResid.get0,2;forintj=0;j1)]TJ/F29 7.9701 Tf 6.5712 0 Td[(Rn/RtMatrixtempWeight=newMatrix1,3,0.0;MatrixtempI=newMatrix1,3,1.0;//tempWeight=Resids.getj.geti.timesTotalResid.inverse;tempWeight.set0,0,Resids.getj.geti.get0,0/TotalResid.get0,0;tempWeight.set0,1,Resids.getj.geti.get1,0/TotalResid.get1,0;tempWeight.set0,2,Resids.getj.geti.get2,0/TotalResid.get2,0;tempWeight=tempI.minustempWeight;TotalWeight=TotalWeight.plustempWeight;Weights.addtempWeight;//System.out.println1)]TJ/F29 7.9701 Tf 7.0683 0 Td[(Rn/Rt;//tempWeight.print3,2;g//System.out.printlnTotalWeight;//TotalWeight.print3,2;MatrixChi=newMatrix1,3,0.0;forintj=0;j
PAGE 164

AppendixAContinued Output.addnewGPSEntryChi.get0,0/tempF.get0,0,Chi.get0,1/tempF.get0,1,Outputs.get0.geti.getGPSTimestamp;MatrixtempR=newMatrix1,3,0.0;GPSEntrygGPS=dDataset.GetGPSEntryi;tempR.set0,0,gGPS.getEastX)]TJ/F16 7.9701 Tf 6.6925 0 Td[(Chi.get0,0/tempF.get0,0;tempR.set0,1,gGPS.getNorthY)]TJ/F16 7.9701 Tf 6.6925 0 Td[(Chi.get0,1/tempF.get0,1;Resid.addtempR;g//Processdone!TimeOff=newDate;TimeElapsed=TimeOff.getTime)]TJ/F16 7.9701 Tf 11.0712 0 Td[(TimeOn.getTime;ProcessOn=false;ProcessDone=true;gpubliclonggetTimeElapsedfreturnTimeElapsed;gpublicVectorgetOutputfifProcessDonefreturnOutput;greturnnull;gpublicVectorgetResidualsf//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedmethodstubreturnnull;gg 152

PAGE 165

AppendixAContinued PF.javapackagefilters;importjava.io.;importjava.util.Date;importjava.util.Random;importjava.util.Vector;importJama.EigenvalueDecomposition;importJama.Matrix;importtestsystem.CompassEntry;importtestsystem.Data;importtestsystem.GPSEntry;importtestsystem.IMUEntry;importtestsystem.OdoEntry;importcontrollers.;publicclassPFimplementsFilterfprivatebooleanProcessOn=false;privatebooleanProcessDone=false;privateDateTimeOn,TimeOff;privateLongTimeElapsed;privateDatadDataset;privateControlcController;privateEKFControlcEController;privateSPKFControlcUController; 153

PAGE 166

AppendixAContinued privateintNumParticles;privateintNumMixParticles;privateVectorOutput=newVector;privateVectorResid=newVector;protectedRandomrand=newRandomnewDate.getTime;staticintL=9;privateintCoreType;privateDoubledR,dQ;privateDoubledefA,defB,defK;privateintNumKLD;privatebooleanResampleStep=false;publicPFControlController,DataDataset,intParticles,ControlDController,intFilterType,DoubleinitQ,DoubleinitRfcController=Controller;dDataset=Dataset;NumParticles=Particles;NumMixParticles=intNumParticles/20;NumKLD=0;dR=initR;dQ=initQ;switchFilterTypefcase1://StandardPFCoreType=1;//FilterType@core[withUKFworking] 154

PAGE 167

AppendixAContinued cUController=SPKFControlDController;this.setAlpha0.02;this.setBeta2.0;this.setKappa0.0;break;case2://EKFPFTypesCoreType=2;//FilterType@core)]TJ/F29 7.9701 Tf 10.5899 0 Td[(EKF![EKFworking]cEController=EKFControlDController;break;case3://UKFPFTypesCoreType=3;//FilterType@core)]TJ/F29 7.9701 Tf 10.3687 0 Td[(UKF![UKFworking]cUController=SPKFControlDController;this.setAlpha0.02;this.setBeta2.0;this.setKappa0.0;break;default:System.out.printlnObviously, something is screwed up!;break;ggpublicPFControlController,DataDataset,intParticles,ControlDController,intFilterType,DoubleinitQ,DoubleinitR,DoubledA,DoubledB,DoubledKfcController=Controller;dDataset=Dataset;NumParticles=Particles;NumMixParticles=intNumParticles/20;dR=initR;dQ=initQ;switchFilterTypefcase1://StandardPF 155

PAGE 168

AppendixAContinued CoreType=1;//FilterType@core[withUKFworking]cUController=SPKFControlDController;this.setAlphadA;this.setBetadB;this.setKappadK;break;case2://EKFPFTypesCoreType=2;//FilterType@core)]TJ/F29 7.9701 Tf 10.5899 0 Td[(EKF![EKFworking]System.out.printlnYou sure you didn't want a UKF)]TJ/F16 7.9701 Tf 5.9182 0 Td[(PF?;cEController=EKFControlDController;break;case3://UKFPFTypesCoreType=3;//FilterType@core)]TJ/F29 7.9701 Tf 10.3687 0 Td[(UKF![UKFworking]cUController=SPKFControlDController;this.setAlphadA;this.setBetadB;this.setKappadK;break;default:System.out.printlnObviously, something is screwed up!;break;ggpublicvoidsetAlphaDoubleAlphafif!ProcessOnf//ChangeAlphacUController.setAlphaAlpha;defA=Alpha;gelseSystem.out.printlnCan't reset Alpha in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;g 156

PAGE 169

AppendixAContinued publicvoidsetBetaDoubleBetafif!ProcessOnf//ChangeBetacUController.setBetaBeta;defB=Beta;gelseSystem.out.printlnCan't reset Beta in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidsetKappaDoubleKappafif!ProcessOnf//ChangeKappacUController.setKappaKappa;defK=Kappa;gelseSystem.out.printlnCan't reset Kappa in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicBooleanfinishedProcessfreturnProcessDone;gpublicvoidoutputDataStringFileNameftryfBufferedWriterout=newBufferedWriternewFileWriterFileName+TimeElapsed+.txt;intlen=Output.size;System.out.printlnTime Elapsed: +TimeElapsed;GPSEntryA;MatrixB;out.write#Timestep EastX NorthY ResidX ResidY ResidZ;out.newLine;forinti=0;i
PAGE 170

AppendixAContinued out.writeA.getGPSTimestamp+ +A.getEastX+ +A.getNorthY+ +B.get0,0+ +B.get1,0+ +B.get2,0;out.newLine;gout.close;gcatchIOExceptionefSystem.out.printlnOutput in PF Blew Up;e.printStackTrace;ggpublicvoidsetControlControlControllerfif!ProcessOnf//ChangetheControllercController=Controller;gelseSystem.out.printlnCan't reset controller in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidsetDataDataDatasetfif!ProcessOnf//ChangetheDatasetdDataset=Dataset;gelseSystem.out.printlnCan't reset data in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidstartProcessf//GeneratenewVariablesGPSEntrygGPS;GPSEntrygOldGPS;IMUEntryiIMU;OdoEntryoOdo;CompassEntrycComp;DoubledTS; 158

PAGE 171

AppendixAContinued VectorvIMU=newVector;VectorvComp=newVector;VectorDeltaH=newVector;VectortempOdo=newVector;VectorvGPS=newVector;VectorvMove=newVector;VectormParticleSet=newVector;VectormixParticleSet=newVector;VectorvGreekHeading=newVector;VectorvGreekHeading1=newVector;VectorvGreekDrive=newVector;VectorvGreekPose=newVector;//Starttheprocess!ProcessOn=true;TimeOn=newDate;//GetthelengthofthedatasetintiLength=dDataset.getLength;//BuildtheFirstStatesvGreekHeading.adddefA;vGreekHeading.adddefB;vGreekHeading.adddefK;vGreekDrive=vGreekPose=vGreekHeading1=vGreekHeading;MatrixHeadingX=newMatrix9,1,0.0;MatrixHeadingP=newMatrix9,9,0.0;MatrixHeading1X=newMatrix9,1,0.0;MatrixHeading1P=newMatrix9,9,0.0;MatrixDriveX=newMatrix9,1,0.0;MatrixDriveP=newMatrix9,9,0.0;MatrixPoseX=newMatrix9,1,0.0;MatrixPoseP=newMatrix9,9,0.0;//SetKalmanMatrixZeroHeadingK=newMatrix6,3,0.0;MatrixHeadingK=newMatrix6,3,0.0; 159

PAGE 172

AppendixAContinued MatrixDriveK=newMatrix6,3,0.0;MatrixPoseK=newMatrix6,3,0.0;//ResetthePvaluestonormHeadingP.set3,3,dQ;HeadingP.set4,4,dQ;HeadingP.set5,5,dQ;HeadingP.set6,6,dR;HeadingP.set7,7,dR;HeadingP.set8,8,dR;Heading1P.set3,3,dQ;Heading1P.set4,4,dQ;Heading1P.set5,5,dQ;Heading1P.set6,6,dR;Heading1P.set7,7,dR;Heading1P.set8,8,dR;DriveP.set3,3,dQ;DriveP.set4,4,dQ;DriveP.set5,5,dQ;DriveP.set6,6,dR;DriveP.set7,7,dR;DriveP.set8,8,dR;PoseP.set3,3,dQ;PoseP.set4,4,dQ;PoseP.set5,5,dQ;PoseP.set6,6,dR;PoseP.set7,7,dR;PoseP.set8,8,dR;gGPS=dDataset.GetGPSEntry0;MatrixtQ=newMatrix1,3,dQ;MatrixtR=newMatrix1,3,dR; 160

PAGE 173

AppendixAContinued //SetKalmanZeroHeadingK.setMatrix3,3,0,2,tQ;ZeroHeadingK.setMatrix4,4,0,2,tR;HeadingK.setMatrix3,3,0,2,tQ;HeadingK.setMatrix4,4,0,2,tR;DriveK.setMatrix3,3,0,2,tQ;DriveK.setMatrix4,4,0,2,tR;PoseK.setMatrix3,3,0,2,tQ;PoseK.setMatrix4,4,0,2,tR;//SetfirstpositionandbuildtheparticlesetgGPS=dDataset.GetGPSEntry0;gOldGPS=gGPS;DoublestartX=gGPS.getEastX;DoublestartY=gGPS.getNorthY;Heading1X.set2,0,dDataset.GetOdoEntry0.getOdoTheta;HeadingX.set2,0,dDataset.GetCompassEntry0.GetRad;DriveX.set0,0,dDataset.GetOdoEntry0.getOdoX;DriveX.set1,0,dDataset.GetOdoEntry0.getOdoY;PoseX.set0,0,startX;PoseX.set1,0,startY;PoseK.set0,0,startX;PoseK.set0,1,startY;//ParticleswillstartwithinaRadiusmeterradiusoftheGPSspot)]TJ/F29 7.9701 Tf -327.0521 -15.7774 Td[(//randomlydistributedDoubleRadius=25.0;MatrixtempParticle=newMatrix3,1;//DEBUG:HeaderforRegularParticlesSystem.out.printlnRegular Particle Set: ;//ENDDEBUGforinti=0;i
PAGE 174

AppendixAContinued //System.out.printlnRandRadius=+RandRadius;tempParticle.set0,0,startX+RandRadius;//System.out.printstartX+RandRadius+,;RandRadius=Radiusrand.nextDouble)]TJ/F16 7.9701 Tf 6.8106 0 Td[(1rand.nextInt2;//System.out.printlnRandRadius=+RandRadius;tempParticle.set1,0,startY+RandRadius;//System.out.printstartY+RandRadius+,;//RandRadius=Radiusrand.nextDouble)]TJ/F29 7.9701 Tf 6.8106 0 Td[(1rand.nextInt2;//System.out.printlnRandRadius=+RandRadius;tempParticle.set2,0,0.0;//DEBUG:ParticleListPostupdatetempParticle.print3,2;//ENDDEBUGmParticleSet.addtempParticle;g//DEBUG:HeaderforMixParticlesSystem.out.printlnMix Particle Set: ;//ENDDEBUGforinti=0;i
PAGE 175

AppendixAContinued //theGPSspot)]TJ/F29 7.9701 Tf 11.6723 0 Td[(Gaussiandistribution//DoubleRadius=10.0;//MatrixtempParticle=newMatrix1,3;//forinti=0;iWeights=newVector;VectorTrueWeights=newVector;DoubleWeight;MatrixtempResid=newMatrix3,1,0.0;DoubleOldTheta=0.0;DoubleOldOdoX=0.0;DoubleOldOdoY=0.0;//Starttheroll!//DEBUGSystem.out.printlnCoreType of PF: +CoreType;//ENDDEBUGforinti=1;i
PAGE 176

AppendixAContinued dTS=gGPS.getGPSTimestamp;ifCoreType==1jjCoreType==3f//Step1:tempOdo.clear;tempOdo.add0.0;tempOdo.add0.0;tempOdo.addoOdo.getOdoTheta)]TJ/F16 7.9701 Tf 11.6731 0 Td[(OldTheta;OldTheta=oOdo.getOdoTheta;vIMU.clear;vIMU.add0.0;vIMU.add0.0;vIMU.addiIMU.getIMURotAccel.get2dTS;SPKFStepHeading1U=newSPKFStepHeading1P,Heading1X,vIMU,tempOdo,vGreekHeading1,dTS;while!Heading1U.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggHeading1P=Heading1U.getP;Heading1X=Heading1U.getX;vGreekHeading1=Heading1U.getGreek;//Step2:vComp.clear;vComp.add0.0;vComp.add0.0;vComp.addcComp.GetRad;tempOdo.clear;tempOdo.addHeading1X.get0,0;tempOdo.addHeading1X.get1,0; 164

PAGE 177

AppendixAContinued tempOdo.addHeading1X.get2,0;SPKFStepHeadingU=newSPKFStepHeadingP,HeadingX,tempOdo,vComp,vGreekHeading,dTS;while!HeadingU.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggHeadingP=HeadingU.getP;HeadingX=HeadingU.getX;vGreekHeading=HeadingU.getGreek;DeltaH.clear;DeltaH.addHeadingX.get0,0;DeltaH.addHeadingX.get1,0;DeltaH.addHeadingX.get2,0;//Step3:vIMU=dDataset.GetIMUVelocityi,DeltaH;forintj=0;j
PAGE 178

AppendixAContinued OldOdoX=oOdo.getOdoX;OldOdoY=oOdo.getOdoY;SPKFStepDriveU=newSPKFStepDriveP,DriveX,vIMU,tempOdo,vGreekDrive,dTS;while!DriveU.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggDriveP=DriveU.getP;DriveX=DriveU.getX;vGreekDrive=DriveU.getGreek;TotalMove=TotalMove.plusDriveX.getMatrix0,2,0,0;////DEBUG:MOVE//System.out.printlnDriveinSPKF:+DriveX.get0,0+//,+DriveX.get1,0+,+DriveX.get2,0;//System.out.printlnTotalMove:+TotalMove.get0,0+,//+TotalMove.get1,0+,+TotalMove.get2,0;////TotalMove.print3,2;////DriveX.getMatrix0,2,0,0.print3,2;////ENDDEBUGforintm=0;m
PAGE 179

AppendixAContinued //tempParticle.get0,0+,+tempParticle.get1,0;mParticleSet.setm,tempParticle;ggelsef//CoreType2!//Step0:vIMU.clear;vIMU.add0.0;vIMU.add0.0;vIMU.addiIMU.getIMURotAccel.get2dTS;tempOdo.clear;tempOdo.add0.0;tempOdo.add0.0;tempOdo.addoOdo.getOdoTheta)]TJ/F16 7.9701 Tf 11.6732 0 Td[(OldTheta;OldTheta=oOdo.getOdoTheta;ZeroHeadingK=KStepZeroHeadingK,tempOdo,vIMU,dTS;//Step1:tempOdo.clear;tempOdo.addZeroHeadingK.get0,0;tempOdo.addZeroHeadingK.get0,1;tempOdo.addZeroHeadingK.get0,2;vComp.clear;vComp.add0.0;vComp.add0.0;vComp.addcComp.GetRad;HeadingK=KStepHeadingK,vComp,tempOdo,dTS;DeltaH.addHeadingK.get0,0;DeltaH.addHeadingK.get0,1;DeltaH.addHeadingK.get0,2; 167

PAGE 180

AppendixAContinued //Step2:vIMU=dDataset.GetIMUVelocityi,DeltaH;forintj=0;j
PAGE 181

AppendixAContinued DoublemixGPSY=gOldGPS.getNorthY+TotalMove.get1,0;DoublemixGPSZ=0.0+TotalMove.get2,0;TotalWeight=0.0;LargestWeight=0.0;TrueWeights.clear;Weights.clear;forintm=0;m
PAGE 182

AppendixAContinued ////ENDDEBUG//Particledrivingdone!ChecktoseeifGPSupdated.doubleOldEast=gOldGPS.getEastX;doubleNewEast=gGPS.getEastX;doubleOldNorth=gOldGPS.getNorthY;doubleNewNorth=gGPS.getNorthY;//ifgOldGPS.getEastX!=gGPS.getEastXjjgOldGPS.getNorthY//!=gGPS.getNorthYfifOldEast!=NewEastjjOldNorth!=NewNorthf//GPSWASUPDATED!//TimeforaCrimeSpree)]TJ/F29 7.9701 Tf 11.5934 0 Td[(orbetteryet,aResamplingStep//thisiswhereMAGICunfoldsResampleStep=true;////DEBUG//System.out.printlnResample!;//System.out.printlnTotalMove:+TotalMove.get0,0+,//+TotalMove.get1,0;//System.out.printlnGPSMove:+NewEast)]TJ/F29 7.9701 Tf 6.603 0 Td[(OldEast+,+//NewNorth)]TJ/F29 7.9701 Tf 6.5399 0 Td[(OldNorth;////ENDDEBUGDoubleGPSX=0.0;DoubleGPSY=0.0;DoubleGPSZ=0.0;MatrixmixDriveSet=newMatrix3,1,0.0;vGPS.clear;vGPS.addgGPS.getEastX;vGPS.addgGPS.getNorthY;vGPS.add0.0;vMove.clear;vMove.addTotalMove.get0,0;vMove.addTotalMove.get1,0;vMove.addTotalMove.get2,0;doubleWeightMod=0.001; 170

PAGE 183

AppendixAContinued TotalWeight=0.0;LargestWeight=0.0;Weights.clear;TrueWeights.clear;ifCoreType==1f//StandardPFLAND!//Steps://1WeightParticles//2Resample!//Easy,no?GPSX=gGPS.getEastX;GPSY=gGPS.getNorthY;Weights.clear;TrueWeights.clear;forintm=0;m
PAGE 184

AppendixAContinued TotalWeight=LargestWeightNumParticles)]TJ/F16 7.9701 Tf 11.7934 0 Td[(TotalWeight+WeightModNumParticles;////DEBUG//System.out.printlnTotalWeight:+TotalWeight;////ENDDEBUGforintm=0;m
PAGE 185

AppendixAContinued forintm=0;m
PAGE 186

AppendixAContinued gTotalWeight=LargestWeightNumParticles)]TJ/F16 7.9701 Tf 11.7934 0 Td[(TotalWeight+WeightModNumParticles;////DEBUG//System.out.printlnTotalWeight:+TotalWeight;////ENDDEBUGforintm=0;m
PAGE 187

AppendixAContinued Thread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggPoseP=PoseU.getP;PoseX=PoseU.getX;vGreekPose=PoseU.getGreek;//FUSIONCOMPLETEGPSX=PoseX.get0,0;GPSY=PoseX.get1,0;GPSZ=PoseX.get2,0;Weights.clear;TrueWeights.clear;forintm=0;m
PAGE 188

AppendixAContinued ////DEBUG//System.out.printlnTotalWeight:+TotalWeight;////ENDDEBUGforintm=0;m
PAGE 189

AppendixAContinued ////DEBUG:ParticleListPostupdate//System.out.printlnActualParticle+m+:;//tempParticle.print3,2;////ENDDEBUGXm+=tempParticle.get0,0;//Weights.getm;Ym+=tempParticle.get1,0;//Weights.getm;Zm+=tempParticle.get2,0;//Weights.getm;gforintm=0;m
PAGE 190

AppendixAContinued //System.out.printlnLocationExtractionPostDivide:+Xm+,+//Ym+,+Zm;tempResid=newMatrix3,1,0.0;tempResid.set0,0,Xm)]TJ/F16 7.9701 Tf 10.8724 0 Td[(gGPS.getEastX;tempResid.set1,0,Ym)]TJ/F16 7.9701 Tf 10.8724 0 Td[(gGPS.getNorthY;Resid.addtempResid;//ifResampleStepf////ParticleNumberManipulation////////DEBUG////System.out.printlnNumberofParticles////Pre:+mParticleSet.size;//////ENDDEBUG//mParticleSet=LikelyhoodBasedAdaptmParticleSet,gGPS,GPSOut,Weights;////DEBUG//System.out.printlnNumberofParticlesPost:+mParticleSet.size;////ENDDEBUG//ResampleStep=false;//g//Tickerifi%1000==0System.out.print.;g//Processdone!TimeOff=newDate;TimeElapsed=TimeOff.getTime)]TJ/F16 7.9701 Tf 11.0713 0 Td[(TimeOn.getTime;ProcessOn=false;ProcessDone=true;g 178

PAGE 191

AppendixAContinued publiclonggetTimeElapsedfreturnTimeElapsed;gprivateVectorLikelyhoodBasedAdaptVectorIncoming,GPSEntryGPSVal,GPSEntryCalcLoc,VectoriWeightsfintM=Incoming.size;VectorOutgoing=Incoming;doubleXOff=CalcLoc.getEastX)]TJ/F16 7.9701 Tf 11.1338 0 Td[(GPSVal.getEastX;doubleYOff=CalcLoc.getNorthY)]TJ/F16 7.9701 Tf 11.1339 0 Td[(GPSVal.getNorthY;doubleDistOff=Math.sqrtXOffXOff+YOffYOff;System.out.printlnDistOff: +DistOff;//Foreverymeterwe'reofftarget,add5particlesrandomlyina//radiusaboutGPSVal,athalfradiusofofftarget//Ifwe'rewithinameter,subtractparticles,killingduplicatesdown//tooneifDistOff>1.2fintNewParts=intDistOff5.0;intNewMixParts=NewParts/20;MatrixtempParticle=newMatrix3,1;forinti=0;i
PAGE 192

AppendixAContinued intRemLoc;ifNumParticles>20f//Makesurewehaveparticlestoremove//Removeallparticleswithlowestpossibleweight//RemLoc=iWeights.indexOf0.0;//whileRemLoc>)]TJ/F29 7.9701 Tf 6.8107 0 Td[(1f//////DEBUG////System.out.printlnRemLoc=+RemLoc;//////ENDDEBUG//Outgoing.removeRemLoc;//iWeights.removeRemLoc;//NumParticles)74()]TJ/F29 7.9701 Tf 12.2846 0 Td[(;//RemLoc=iWeights.indexOf0.0;//g//RemoveallparticleswithaweightlessthanCullPointdoubleCullPoint=0.1;forinti=0;iLowVarianceResampleVectorIncoming,VectoriWeights,DoubleTWeightf 180

PAGE 193

AppendixAContinued intM=Incoming.size;//System.out.printlnIncomingSize:+M;//System.out.printlniWeightsSize:+iWeights.size;Doubler=rand.nextDouble1.0/doubleM;//System.out.printlnr:+r;Doublec=iWeights.get0/TWeight;DoubleU;inti=1;VectorOutgoing=newVector;forintm=1;m<=M;m++fU=r+doublem)]TJ/F16 7.9701 Tf 12.0695 0 Td[(1.0/doubleM;//System.out.printlnU=+U;//System.out.printlnm=+m;whileU>cf////DEBUG//System.out.printlniinLVR:+i;//System.out.printlnc@i=+c;////ENDDEBUGi++;c+=iWeights.geti)]TJ/F16 7.9701 Tf 11.7422 0 Td[(1/TWeight;gOutgoing.addIncoming.geti)]TJ/F16 7.9701 Tf 11.7422 0 Td[(1;g//System.out.printlnOutgoingSize:+Outgoing.size;returnOutgoing;gprivateVectorWRSVectorIncoming,VectoriWeights,DoubleTWeightfVectorOutgoing=newVector;intk=Incoming.size;forinti=0;i
PAGE 194

AppendixAContinued ki=Math.powki,1/iWeights.get0;DoubleTw=ki;DoubleRunWeight=0.0;intChoice=0;forintj=0;jKLDVectorIncoming,GPSEntryNewGPS,VectoriWeights,DoubleTWeightfVectorOutgoing=newVector;intiSize=Incoming.size;DoubleNewX=NewGPS.getEastX;DoubleNewY=NewGPS.getNorthY; 182

PAGE 195

AppendixAContinued ////DEBUG//NumKLD++;//System.out.printlnNumParticlesPreKLD:+NumParticles;////ENDDEBUG//BuildBaboutGPS)]TJ/F29 7.9701 Tf 11.8631 0 Td[(0.5mx0.5mxpi/12radoutto10m//Starts@EastY=0,X=0.5nmandrotatesCCWbypi/12rad//intervals.VectorB=newVector;forinti=0;i<480;i++fB.add0;gintM=0;intMx=intdoubleiSize1.5;//100000;intk=0;Doublez=2.33;Doublee=0.05;dofDoubleki=rand.nextDouble;ki=Math.powki,1/iWeights.get0;DoubleTw=ki;DoubleRunWeight=0.0;intChoice=0;forintj=0;j
PAGE 196

AppendixAContinued RunWeight=RunWeight+iWeights.getj;ggMatrixtempParticle=Incoming.getChoice;Outgoing.addtempParticle;//SolveforbinningDoubleTempX=NewX)]TJ/F16 7.9701 Tf 12.0823 0 Td[(tempParticle.get0,0;DoubleTempY=NewY)]TJ/F16 7.9701 Tf 12.0823 0 Td[(tempParticle.get1,0;VectortempPolar=CartToPolarTempX,TempY;DoubleTempRads=tempPolar.get1;ifTempY<0TempRads+=Math.PI;DoubleTempDist=tempPolar.get0;intBinNum=0;intBinRing=0;intBinRad=0;ifTempDist>10f//Outsideour10meterrange,maxbinradiusBinRing=19;gelsefBinRing=intTempDist/0.5;gBinNum=BinRing24;BinRad=intTempRads/Math.PI/12;BinNum+=BinRad;//System.out.printlnBinNum:+BinNum;ifB.getBinNum==0f//BinisEmpty!//System.out.printlnBinisEmpty;k++;B.setBinNum,1;ifk>1fMx=intk)]TJ/F16 7.9701 Tf 11.7422 0 Td[(1/2eMath.pow1)]TJ/F16 7.9701 Tf 11.7422 0 Td[(2/9k)]TJ/F16 7.9701 Tf 11.7422 0 Td[(1+Math.sqrt2/9k)]TJ/F16 7.9701 Tf 11.7422 0 Td[(1z,3;//System.out.printlnMxUpdate:+Mx; 184

PAGE 197

AppendixAContinued ggM=M+1;gwhileM1f////BufferedReaderin=newBufferedReadernewFileReaderNumKLD)]TJ/F29 7.9701 Tf 11.7422 0 Td[(1+////.csv;////StringTempStr;////inti=0;////whileTempStr=in.readLine!=nullf////ifi
PAGE 198

AppendixAContinued ////g////out.writeOutgoing.geti.get0,0+,+Outgoing.geti.get1,////0+,;////out.newLine;////g////gelsef////forinti=0;iPolarToCartDoubleDist,DoubleRadsfVectorCart=newVector;//polartoCartesiandoublex=Math.cosRadsDist;doubley=Math.sinRadsDist;Cart.addx;Cart.addy;returnCart; 186

PAGE 199

AppendixAContinued gprivateVectorCartToPolarDoubleX,DoubleYfVectorPole=newVector;//Cartesiantopolar.doubleradius=Math.sqrtXX+YY;doubleangleInRadians=Math.acosX/radius;Pole.addradius;Pole.addangleInRadians;returnPole;gprivateclassSPKFStepfprivateMatrixP;privateMatrixX;privateMatrixResidual;privateDoubledTimestep;privateVectordControl;privateVectordMeasure;privateVectordGreek;privatebooleanStepDone=false;voidsetPMatrixPDashfP=PDash;gvoidsetXMatrixXDashf 187

PAGE 200

AppendixAContinued X=XDash;gMatrixgetPfreturnP;gMatrixgetXfreturnX;gMatrixgetResidfreturnResidual;gVectorgetGreekfreturndGreek;gbooleanStepFinishedfreturnStepDone;gSPKFStepMatrixPDash,MatrixXDash,VectorControl,VectorMeasurement,VectorGreek,DoubledInTimestepfP=PDash;X=XDash;dGreek=Greek;dTimestep=dInTimestep;dControl=Control;dMeasure=Measurement;RunSPKF;gprivateMatrixSqrtSPKFMatrixPDashf//OnlyworkswithasymmetricPositiveDefinitematrix 188

PAGE 201

AppendixAContinued //[V,D]=eigA//S=VdiagsqrtdiagDV'//////PDashin//System.out.printlnPDash:;//PDash.print3,2;//MatrixNegKeeper=Matrix.identity9,9;////TestingForcedCompliance//forinti=0;i<9;i++f//ifPDash.geti,i<0f//NegKeeper.seti,i,)]TJ/F29 7.9701 Tf 7.2383 0 Td[(1;//PDash.seti,i,)]TJ/F29 7.9701 Tf 6.3544 0 Td[(PDash.geti,i;//g//gEigenvalueDecompositioneig=PDash.eig;MatrixV=eig.getV;MatrixD=eig.getD;intiDSize=D.getRowDimension;forinti=0;i
PAGE 202

AppendixAContinued //Lineupinitialvariablesfromthecontroller!DoubledAlpha=dGreek.get0;DoubledBeta=dGreek.get1;cUController.setAlphadAlpha;cUController.setBetadBeta;cUController.setKappadGreek.get2;DoubledGamma=cUController.getGamma;DoubledLambda=cUController.getLambda;////DEBUG)]TJ/F29 7.9701 Tf 12.0212 0 Td[(PrinttheGreeks//System.out.printlnGreeks!;//System.out.printlnAlpha)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dAlpha;//System.out.printlnBeta)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dBeta;//System.out.printlnKappa)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dGreek.get2;//System.out.printlnLambda)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dLambda;//System.out.printlnGamma)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dGamma;//Let'sgetstarted://Step1:GenerateTestPointsVectorChi=newVector;VectorUpChi=newVector;VectorUpY=newVector;MatrixUpPx=newMatrix3,3,0.0;MatrixUpPy=newMatrix3,3,0.0;MatrixUpPxy=newMatrix3,3,0.0;MatrixK;Vectorwc=newVector;Vectorwm=newVector;Chi.addX;//AddChi 0)]TJ/F29 7.9701 Tf 11.9189 0 Td[(thecurrentstateestimateX,Y,Z//BigPMatrixisLxLdiagonalMatrixSqrtP=SqrtSPKFP;SqrtP=SqrtP.timesdGamma;//SetupSigmaPointsforinti=0;i<=8;i++f 190

PAGE 203

AppendixAContinued MatrixtempVec=SqrtP.getMatrix0,8,i,i;MatrixtempX=X;MatrixtempPlus=tempX.plustempVec;//System.out.printlnTempPlus;//tempPlus.print3,2;MatrixtempMinu=tempX.minustempVec;//System.out.printlnTempMinus;//tempMinu.print3,2;//tempX=X.copy;//tempX.setMatrixi,i,0,2,tempPlus;Chi.addtempPlus;//tempX=X.copy;//tempX.setMatrixi,i,0,2,tempMinu;Chi.addtempMinu;g//DEBUGPrintthelinesinsidetheChiMatrix2LxL//forinti=0;i<=2L;i++f//System.out.printlnChiMatrixSet:+i;//Chi.geti.print5,2;//g//GenerateweightsDoubleWeightZero=dLambda/L+dLambda;DoubleOtherWeight=1/2L+dLambda;DoubleTotalWeight=WeightZero;wm.addWeightZero;wc.addWeightZero+1)]TJ/F16 7.9701 Tf 11.8312 0 Td[(dAlphadAlpha+dBeta;forinti=1;i<=2L;i++fTotalWeight=TotalWeight+OtherWeight;wm.addOtherWeight;wc.addOtherWeight;g//WeightsMUSTBE1intotalforinti=0;i<=2L;i++fwm.seti,wm.geti/TotalWeight; 191

PAGE 204

AppendixAContinued wc.seti,wc.geti/TotalWeight;g////DEBUGPrinttheweights//System.out.printlnTotalWeight:;//System.out.printlnTotalWeight;//forinti=0;i<=2L;i++f//System.out.printlnWeightMfor+i+Entry;//System.out.printlnwm.geti;//System.out.printlnWeightCfor+i+Entry;//System.out.printlnwc.geti;//g//Step2:PropagateTestPoints//ThiswillalsohandlecomputingthemeanDoubleux=dControl.elementAt0;Doubleuy=dControl.elementAt1;Doubleuz=dControl.elementAt2;MatrixXhatMean=newMatrix3,1,0.0;forinti=0;i
PAGE 205

AppendixAContinued //System.out.printlnChixMinus:;//Chixminus.print3,2;UpChi.addChixminus;XhatMean=XhatMean.plusChixminus.timeswm.geti;g//Meanisright!//System.out.printlnXhatMean:;//XhatMean.print3,2;//Step3:ComputePredictedMeanandCovariance//Welp,wealreadysolvedthemean)]TJ/F29 7.9701 Tf 12.3613 0 Td[(let'sdothecovariancenowforinti=0;i<=2L;i++fMatrixtempP=UpChi.geti.minusXhatMean;MatrixtempPw=tempP.timeswc.geti;tempP=tempPw.timestempP.transpose;UpPx=UpPx.plustempP;g//NewSteps!//Step4:ComputeMeasurements!andYmean!MatrixYhatMean=newMatrix3,1,0.0;forinti=0;i<=2L;i++fMatrixChiOne=Chi.geti;MatrixChiyminus=newMatrix3,1,0.0;DoubleXhat=UpChi.geti.get0,0;DoubleYhat=UpChi.geti.get1,0;DoubleZhat=UpChi.geti.get2,0;DoubleXerr=ChiOne.get6,0;DoubleYerr=ChiOne.get7,0;DoubleZerr=ChiOne.get8,0;Xhat=Xhat+Xerr;Yhat=Yhat+Yerr; 193

PAGE 206

AppendixAContinued Zhat=Zhat+Zerr;Chiyminus.set0,0,Xhat;Chiyminus.set1,0,Yhat;Chiyminus.set2,0,Zhat;UpY.addChiyminus;YhatMean=YhatMean.plusChiyminus.timeswm.geti;g////Welp,wealreadysolvedthemean)]TJ/F29 7.9701 Tf 12.3613 0 Td[(let'sdothecovariances//now//System.out.printlnXHatMeanandYHatMean=;//XhatMean.print3,2;//YhatMean.print3,2;forinti=0;i<=2L;i++fMatrixtempPx=UpChi.geti.minusXhatMean;MatrixtempPy=UpY.geti.minusYhatMean;//System.out.printlnChiX)]TJ/F29 7.9701 Tf 11.3287 0 Td[(XhatMeanandChiY)]TJ/F29 7.9701 Tf 6.2945 0 Td[(YhatMean;//tempPx.print3,2;//tempPy.print3,2;MatrixtempPxw=tempPx.timeswc.geti;MatrixtempPyw=tempPy.timeswc.geti;tempPx=tempPxw.timestempPy.transpose;tempPy=tempPyw.timestempPy.transpose;UpPy=UpPy.plustempPy;UpPxy=UpPxy.plustempPx;g//Step6:ComputeCorrectionsandUpdate//ComputeKalmanGain!//System.out.printlnUpdatedPx;//UpPx.print5,2; 194

PAGE 207

AppendixAContinued //System.out.printlnUpdatedPy;//UpPy.print5,2;//System.out.printlnUpdatedPxy;//UpPxy.print5,2;K=UpPxy.timesUpPy.inverse;//System.out.printlnKalman;//K.print5,2;MatrixMea=newMatrix3,1,0.0;Mea.set0,0,dMeasure.get0;Mea.set1,0,dMeasure.get1;Mea.set2,0,dMeasure.get2;MatrixOut=K.timesMea.minusYhatMean;Out=Out.plusXhatMean;//System.out.printlnOut:;//Out.print3,2;MatrixPx=UpPx.minusK.timesUpPy.timesK.transpose;//UpdateStuff!//PushthePtothecontrollerMatrixOutP=P.copy;OutP.setMatrix0,2,0,2,Px;X.setMatrix0,2,0,0,Out;Residual=XhatMean.minusOut;cUController.inputStateOutP,Residual;//cController.setLL;cUController.startProcess;while!cUController.finishedProcessftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace; 195

PAGE 208

AppendixAContinued ggdGreek.set0,cUController.getAlpha;dGreek.set1,cUController.getBeta;dGreek.set2,cUController.getKappa;P=cUController.getP;StepDone=true;ggprivateMatrixKStepMatrixvOldState,VectordMeasurement,VectordU,DoubledTimeStepf//BigKalman)]TJ/F29 7.9701 Tf 12.1758 0 Td[(individualdRanddQ//Vector>vOutput=newVector>;//ExtractallofthenecessaryvariablesDoubleXKMinus=vOldState.get0,0;DoubleYKMinus=vOldState.get0,1;DoubleZKMinus=vOldState.get0,2;DoubleXPkminus=vOldState.get2,0;DoubleYPkminus=vOldState.get2,1;DoubleZPkminus=vOldState.get2,2;DoubledQx=vOldState.get3,0;DoubledQy=vOldState.get3,1;DoubledQz=vOldState.get3,2;DoubledRx=vOldState.get4,0;DoubledRy=vOldState.get4,1;DoubledRz=vOldState.get4,2;//Doublealphax=vOldState.get5,0;//Doublealphay=vOldState.get5,1;//Doublealphaz=vOldState.elementAt5.elementAt2; 196

PAGE 209

AppendixAContinued //ProjecttheNewStateaheaddrive!DoubleXMinus=XKMinus+dU.get0;DoubleYMinus=YKMinus+dU.get1;DoubleZMinus=ZKMinus+dU.get2;//DoubleZMinus=ZKMinus+dStateUpdate[3]dTimeStep;//ProjecttheErrorCovarianceAheadDoubleXPMinus=XPkminus+dQx;DoubleYPMinus=YPkminus+dQy;DoubleZPMinus=ZPkminus+dQz;//DoubleZPMinus=alphazalphazZPkminusalphazalphaz+//dQz;//ComputeKalmanGainDoubleXKal=XPMinus/XPMinus+dRx;DoubleYKal=YPMinus/YPMinus+dRy;DoubleZKal=ZPMinus/ZPMinus+dRz;//DoubleZKal=ZPMinus/ZPMinus+dRz/alphazalphaz;//UpdateEstimatewithmeasurementDoubleX=XMinus+XKaldMeasurement.get0)]TJ/F16 7.9701 Tf 11.1338 0 Td[(XMinus;DoubleY=YMinus+YKaldMeasurement.get1)]TJ/F16 7.9701 Tf 11.1338 0 Td[(YMinus;DoubleZ=ZMinus+ZKaldMeasurement.get2)]TJ/F16 7.9701 Tf 11.2602 0 Td[(ZMinus;//UpdatetheErrorCovarianceDoublePX=1)]TJ/F16 7.9701 Tf 11.1402 0 Td[(XKal/XPMinus;DoublePY=1)]TJ/F16 7.9701 Tf 11.1402 0 Td[(YKal/YPMinus;DoublePZ=1)]TJ/F16 7.9701 Tf 11.3172 0 Td[(ZKal/ZPMinus;//OutputMatrixOut=vOldState;Out.set0,0,X;Out.set0,1,Y;Out.set0,2,Z;Out.set1,0,XMinus;Out.set1,1,YMinus; 197

PAGE 210

AppendixAContinued Out.set1,2,ZMinus;Out.set2,0,PX;Out.set2,1,PY;Out.set2,2,PZ;//VectorRowZero=newVector;//RowZero.addX;//RowZero.addY;////RowZero.addZ;////NOTOUTPUT...=0//VectorRowOne=newVector;//RowOne.addXMinus;//RowOne.addYMinus;////RowOne.addZMinus;//VectorRowTwo=newVector;//RowTwo.addPX;//RowTwo.addPY;////RowTwo.addPZ;////vOutput.addRowZero;//vOutput.addRowOne;//vOutput.addRowTwo;//vOutput.addvOldState.elementAt3;//vOutput.addvOldState.elementAt4;//vOutput.addvOldState.elementAt5;//UpdatealphaandR/QcEController.inputStateOut;cEController.startProcess;while!cEController.finishedProcessftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;g 198

PAGE 211

AppendixAContinued gOut.set3,0,cEController.getQ.get0;Out.set3,1,cEController.getQ.get1;Out.set4,0,cEController.getR.get0;Out.set4,1,cEController.getR.get1;Out.set5,0,cEController.getAlpha.get0;Out.set5,1,cEController.getAlpha.get1;//vOutput.remove3;//vOutput.remove3;//vOutput.remove3;//vOutput.addcEController.getQ;//vOutput.addcEController.getR;//vOutput.addcEController.getAlpha;returnOut;gpublicVectorgetOutputfifProcessDonefreturnOutput;greturnnull;gpublicVectorgetResidualsfifProcessDonefreturnResid;greturnnull;gg 199

PAGE 212

AppendixAContinued SPKF.javapackagefilters;importjava.util.Date;importjava.util.Random;importjava.util.Vector;importJama.;importjava.io.BufferedWriter;importjava.io.FileWriter;importjava.io.IOException;importjava.lang.Math;importtestsystem.CompassEntry;importtestsystem.Data;importtestsystem.GPSEntry;importtestsystem.IMUEntry;importtestsystem.OdoEntry;importcontrollers.;publicclassSPKFimplementsFilterf/@paramargs/privatebooleanProcessOn=false;privatebooleanProcessDone=false;privateDateTimeOn,TimeOff;privateLongTimeElapsed;privateDatadDataset; 200

PAGE 213

AppendixAContinued privateDoubledQ,dR;protectedSPKFControlcController;protectedFuzzySPKFcFController;privateVectorOutput=newVector;privateVectorResid=newVector;//privateVectorOutputH=newVector;////privateVectorResidH=newVector;////privateVectorOutputH1=newVector;////privateVectorResidH1=newVector;////privateVectorOutputD=newVector;////privateVectorResidD=newVector;privateDoubledefA,defB,defK;//SpecificVariablesprotectedstaticintL=9;//ListheDimensionoftheStatesX,Y,Z,//plusthedimensionofthetwoerrorspublicSPKFControlController,DataDataset,DoubleinitQ,DoubleinitRfcController=SPKFControlController;ifcController.getControlType==2cFController=FuzzySPKFController;dDataset=Dataset;this.setAlpha0.02; 201

PAGE 214

AppendixAContinued this.setBeta2.0;this.setKappa0.0;cController.setLL;dQ=initQ;dR=initR;gpublicSPKFControlController,DataDataset,DoubleinitQ,DoubleinitR,DoubleAlpha,DoubleBeta,DoubleKappafcController=SPKFControlController;dDataset=Dataset;this.setAlphaAlpha;this.setBetaBeta;this.setKappaKappa;cController.setLL;dQ=initQ;dR=initR;gpublicBooleanfinishedProcessfreturnProcessDone;gpublicvoidoutputDataStringFileNameftryfBufferedWriterout=newBufferedWriternewFileWriterFileName+TimeElapsed+.txt;//BufferedWriteroutH1=newBufferedWriternewFileWriterFileName//+HeadingOne;//BufferedWriteroutH=newBufferedWriternewFileWriterFileName//+Heading;//BufferedWriteroutD=newBufferedWriternewFileWriterFileName//+Drive;intlen=Output.size;System.out.printlnTime Elapsed: +TimeElapsed;GPSEntryA; 202

PAGE 215

AppendixAContinued MatrixB;out.write#Timestep EastX NorthY ResidX ResidY ResidZ;out.newLine;forinti=0;i
PAGE 216

AppendixAContinued //outD.writeTimestep,DriveX,DriveY,DriveDelta,ResidX,ResidY,//ResidZ;//outD.newLine;//forinti=0;i
PAGE 217

AppendixAContinued publicvoidsetKappaDoubleKappafif!ProcessOnf//ChangeKappacController.setKappaKappa;defK=Kappa;gelseSystem.out.printlnCan't reset Kappa in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidsetControlControlControllerfif!ProcessOnf//ChangetheControllercController=SPKFControlController;gelseSystem.out.printlnCan't reset controller in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidsetDataDataDatasetfif!ProcessOnf//ChangetheDatasetdDataset=Dataset;gelseSystem.out.printlnCan't reset data in mid)]TJ/F16 7.9701 Tf 6.8809 0 Td[(process!;gpublicvoidstartProcessf//GeneratenewVariablesGPSEntrygGPS;IMUEntryiIMU;OdoEntryoOdo;CompassEntrycComp;DoubledTS;VectorvIMU=newVector;VectorvComp=newVector;VectorDeltaH=newVector; 205

PAGE 218

AppendixAContinued VectortempOdo=newVector;VectorvGPS=newVector;VectorvGreekHeading=newVector;VectorvGreekHeading1=newVector;MatrixDeviation=newMatrix2,1,0.0;//StoresAlpha,//Betaand//Kappafor//eachtypeof//controllerVectorvGreekDrive=newVector;//StoresAlpha,//BetaandKappa//foreachtypeof//controllerVectorvGreekPose=newVector;//StoresAlpha,//BetaandKappa//foreachtypeof//controller//Starttheprocess!ProcessOn=true;TimeOn=newDate;//GetthelengthofthedatasetintiLength=dDataset.getLength;//BuildtheFirstStatesvGreekHeading.adddefA;vGreekHeading.adddefB;vGreekHeading.adddefK;vGreekDrive=vGreekPose=vGreekHeading1=vGreekHeading;MatrixHeadingX=newMatrix9,1,0.0;MatrixHeadingP=newMatrix9,9,0.0;MatrixHeading1X=newMatrix9,1,0.0;MatrixHeading1P=newMatrix9,9,0.0; 206

PAGE 219

AppendixAContinued MatrixDriveX=newMatrix9,1,0.0;MatrixDriveP=newMatrix9,9,0.0;MatrixPoseX=newMatrix9,1,0.0;MatrixPoseP=newMatrix9,9,0.0;//ResetthePvaluestonormHeadingP.set3,3,dQ;HeadingP.set4,4,dQ;HeadingP.set5,5,dQ;HeadingP.set6,6,dR;HeadingP.set7,7,dR;HeadingP.set8,8,dR;Heading1P.set3,3,dQ;Heading1P.set4,4,dQ;Heading1P.set5,5,dQ;Heading1P.set6,6,dR;Heading1P.set7,7,dR;Heading1P.set8,8,dR;DriveP.set3,3,dQ;DriveP.set4,4,dQ;DriveP.set5,5,dQ;DriveP.set6,6,dR;DriveP.set7,7,dR;DriveP.set8,8,dR;PoseP.set3,3,dQ;PoseP.set4,4,dQ;PoseP.set5,5,dQ;PoseP.set6,6,2.5;PoseP.set7,7,2.5;PoseP.set8,8,2.5;gGPS=dDataset.GetGPSEntry0;Heading1X.set2,0,dDataset.GetOdoEntry0.getOdoTheta; 207

PAGE 220

AppendixAContinued HeadingX.set2,0,dDataset.GetCompassEntry0.GetRad;DriveX.set0,0,dDataset.GetOdoEntry0.getOdoX;DriveX.set1,0,dDataset.GetOdoEntry0.getOdoY;PoseX.set0,0,gGPS.getEastX;PoseX.set1,0,gGPS.getNorthY;DoubleOldTheta=0.0;DoubleOldGPSX=gGPS.getEastX;DoubleOldGPSY=gGPS.getNorthY;DoubleOldOdoX=0.0;DoubleOldOdoY=0.0;DoubleOldComp=dDataset.GetCompassEntry0.GetRad;//Starttheroll!forinti=0;i
PAGE 221

AppendixAContinued tempOdo.add0.0;tempOdo.add0.0;tempOdo.addoOdo.getOdoTheta)]TJ/F16 7.9701 Tf 11.6731 0 Td[(OldTheta;OldTheta=oOdo.getOdoTheta;vIMU.clear;vIMU.add0.0;vIMU.add0.0;vIMU.addiIMU.getIMURotAccel.get2dTS;SPKFStepHeadingOne=newSPKFStepHeading1P,Heading1X,vIMU,tempOdo,vGreekHeading1,dTS;while!HeadingOne.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggHeading1P=HeadingOne.getP;Heading1X=HeadingOne.getX;vGreekHeading1=HeadingOne.getGreek;//OutputH1.addnewCompassEntryHeading1X.get2,0,dTS,true;//ResidH1.addHeadingOne.getResid;//Step2:vComp.clear;vComp.add0.0;vComp.add0.0;vComp.addcComp.GetRad;tempOdo.clear;tempOdo.addHeading1X.get0,0;tempOdo.addHeading1X.get1,0;tempOdo.addHeading1X.get2,0;//System.out.printlnHeadingBeforeatStep+i+:; 209

PAGE 222

AppendixAContinued //HeadingP.print3,2;//HeadingX.print3,2;SPKFStepHeading=newSPKFStepHeadingP,HeadingX,tempOdo,vComp,vGreekHeading,dTS;while!Heading.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggHeadingP=Heading.getP;HeadingX=Heading.getX;vGreekHeading=Heading.getGreek;//OutputH.addnewCompassEntryHeadingX.get2,0,dTS,true;//ResidH.addHeading.getResid;//System.out.printlnHeadingAfteratStep+i+:;//HeadingP.print3,2;//HeadingX.print3,2;DeltaH.clear;DeltaH.addHeadingX.get0,0;DeltaH.addHeadingX.get1,0;DeltaH.addHeadingX.get2,0;//Step3:vIMU=dDataset.GetIMUVelocityi,DeltaH;forintj=0;j
PAGE 223

AppendixAContinued OldOdoX=oOdo.getOdoX;OldOdoY=oOdo.getOdoY;//////System.out.printlnDriveBeforeatStep+i+:;//DriveP.print3,2;//DriveX.print3,2;SPKFStepDrive=newSPKFStepDriveP,DriveX,vIMU,tempOdo,vGreekDrive,dTS;while!Drive.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggDriveP=Drive.getP;DriveX=Drive.getX;vGreekDrive=Drive.getGreek;//OutputD.addnewOdoEntryDriveX.get0,0,DriveX.get1,0,//DeltaH.get2,dTS;//ResidD.addDrive.getResid;//System.out.printlnDriveAfteratStep+i+:;//DriveP.print3,2;//DriveX.print3,2;//Step4:tempOdo.clear;tempOdo.addDriveX.get0,0;tempOdo.addDriveX.get1,0;tempOdo.addDriveX.get2,0;vGPS.clear;vGPS.addgGPS.getEastX;//)]TJ/F29 7.9701 Tf 6.2402 0 Td[(OldGPSX;vGPS.addgGPS.getNorthY;//)]TJ/F29 7.9701 Tf 6.2949 0 Td[(OldGPSY;vGPS.add0.0;ifcController.getControlType==2fforintj=0;j
PAGE 224

AppendixAContinued vIMU.setj,vIMU.getjdTS;gDeviation.set0,0,gGPS.getEastX)]TJ/F16 7.9701 Tf 12.1859 0 Td[(vIMU.get0dTS+OldGPSX;Deviation.set1,0,gGPS.getNorthY)]TJ/F16 7.9701 Tf 12.1859 0 Td[(vIMU.get1dTS+OldGPSY;cFController=FuzzySPKFcController;cFController.isGPSDeviation;cController=cFController;gOldGPSX=gGPS.getEastX;OldGPSY=gGPS.getNorthY;//System.out.printlnPoseBeforeatStep+i+:;//PoseP.print3,2;//PoseX.print3,2;SPKFStepPose=newSPKFStepPoseP,PoseX,tempOdo,vGPS,vGreekPose,dTS;while!Pose.StepFinishedftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;ggPoseP=Pose.getP;PoseX=Pose.getX;vGreekPose=Pose.getGreek;//System.out.printlnGPS:+gGPS.getEastX+,+//gGPS.getNorthY;//System.out.printlnPoseAfteratStep+i+:;//PoseP.print3,2;//PoseX.print3,2;GPSEntryGPSOut=newGPSEntryPoseX.get0,0,PoseX.get1,0,dTS;//System.out.printlnGPSOut.toString;Output.addGPSOut;Resid.addPose.getResid; 212

PAGE 225

AppendixAContinued //Tickerifi%100==0System.out.print.;g//Processdone!TimeOff=newDate;TimeElapsed=TimeOff.getTime)]TJ/F16 7.9701 Tf 11.0713 0 Td[(TimeOn.getTime;ProcessOn=false;ProcessDone=true;gpubliclonggetTimeElapsedfreturnTimeElapsed;gprivateclassSPKFStepfprivateMatrixP;privateMatrixX;privateMatrixResidual;privateDoubledTimestep;privateVectordControl;privateVectordMeasure;privateVectordGreek;privatebooleanStepDone=false;voidsetPMatrixPDashfP=PDash; 213

PAGE 226

AppendixAContinued gvoidsetXMatrixXDashfX=XDash;gMatrixgetPfreturnP;gMatrixgetXfreturnX;gMatrixgetResidfreturnResidual;gVectorgetGreekfreturndGreek;gbooleanStepFinishedfreturnStepDone;gSPKFStepMatrixPDash,MatrixXDash,VectorControl,VectorMeasurement,VectorGreek,DoubledInTimestepfP=PDash;X=XDash;dGreek=Greek;dTimestep=dInTimestep;dControl=Control;dMeasure=Measurement;RunSPKF;g 214

PAGE 227

AppendixAContinued privateMatrixSqrtSPKFMatrixPDashf//OnlyworkswithasymmetricPositiveDefinitematrix//[V,D]=eigA//S=VdiagsqrtdiagDV'//////PDashin//System.out.printlnPDash:;//PDash.print3,2;//MatrixNegKeeper=Matrix.identity9,9;////TestingForcedCompliance//forinti=0;i<9;i++f//ifPDash.geti,i<0f//NegKeeper.seti,i,)]TJ/F29 7.9701 Tf 7.2383 0 Td[(1;//PDash.seti,i,)]TJ/F29 7.9701 Tf 6.3544 0 Td[(PDash.geti,i;//g//gEigenvalueDecompositioneig=PDash.eig;MatrixV=eig.getV;MatrixD=eig.getD;intiDSize=D.getRowDimension;forinti=0;i
PAGE 228

AppendixAContinued //5ComputeInnovationsandCrossCovariance//6Computecorrectionsandupdate//Lineupinitialvariablesfromthecontroller!DoubledAlpha=dGreek.get0;DoubledBeta=dGreek.get1;cController.setAlphadAlpha;cController.setBetadBeta;cController.setKappadGreek.get2;DoubledGamma=cController.getGamma;DoubledLambda=cController.getLambda;////DEBUG)]TJ/F29 7.9701 Tf 12.0212 0 Td[(PrinttheGreeks//System.out.printlnGreeks!;//System.out.printlnAlpha)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dAlpha;//System.out.printlnBeta)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dBeta;//System.out.printlnKappa)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dGreek.get2;//System.out.printlnLambda)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dLambda;//System.out.printlnGamma)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+dGamma;//Let'sgetstarted://Step1:GenerateTestPointsVectorChi=newVector;VectorUpChi=newVector;VectorUpY=newVector;MatrixUpPx=newMatrix3,3,0.0;MatrixUpPy=newMatrix3,3,0.0;MatrixUpPxy=newMatrix3,3,0.0;MatrixK;Vectorwc=newVector;Vectorwm=newVector;Chi.addX;//AddChi 0)]TJ/F29 7.9701 Tf 11.9189 0 Td[(thecurrentstateestimateX,Y,Z//BigPMatrixisLxLdiagonalMatrixSqrtP=SqrtSPKFP;SqrtP=SqrtP.timesdGamma; 216

PAGE 229

AppendixAContinued //SetupSigmaPointsforinti=0;i<=8;i++fMatrixtempVec=SqrtP.getMatrix0,8,i,i;MatrixtempX=X;MatrixtempPlus=tempX.plustempVec;//System.out.printlnTempPlus;//tempPlus.print3,2;MatrixtempMinu=tempX.minustempVec;//System.out.printlnTempMinus;//tempMinu.print3,2;//tempX=X.copy;//tempX.setMatrixi,i,0,2,tempPlus;Chi.addtempPlus;//tempX=X.copy;//tempX.setMatrixi,i,0,2,tempMinu;Chi.addtempMinu;g//DEBUGPrintthelinesinsidetheChiMatrix2LxL//forinti=0;i<=2L;i++f//System.out.printlnChiMatrixSet:+i;//Chi.geti.print5,2;//g//GenerateweightsDoubleWeightZero=dLambda/L+dLambda;DoubleOtherWeight=1/2L+dLambda;DoubleTotalWeight=WeightZero;wm.addWeightZero;wc.addWeightZero+1)]TJ/F16 7.9701 Tf 11.8312 0 Td[(dAlphadAlpha+dBeta;forinti=1;i<=2L;i++fTotalWeight=TotalWeight+OtherWeight;wm.addOtherWeight;wc.addOtherWeight;g 217

PAGE 230

AppendixAContinued //WeightsMUSTBE1intotalforinti=0;i<=2L;i++fwm.seti,wm.geti/TotalWeight;wc.seti,wc.geti/TotalWeight;g////DEBUGPrinttheweights//System.out.printlnTotalWeight:;//System.out.printlnTotalWeight;//forinti=0;i<=2L;i++f//System.out.printlnWeightMfor+i+Entry;//System.out.printlnwm.geti;//System.out.printlnWeightCfor+i+Entry;//System.out.printlnwc.geti;//g//Step2:PropagateTestPoints//ThiswillalsohandlecomputingthemeanDoubleux=dControl.elementAt0;Doubleuy=dControl.elementAt1;Doubleuz=dControl.elementAt2;MatrixXhatMean=newMatrix3,1,0.0;forinti=0;i
PAGE 231

AppendixAContinued Chixminus.set0,0,Xhat;Chixminus.set1,0,Yhat;Chixminus.set2,0,Zhat;//System.out.printlnChixMinus:;//Chixminus.print3,2;UpChi.addChixminus;XhatMean=XhatMean.plusChixminus.timeswm.geti;g//Meanisright!//System.out.printlnXhatMean:;//XhatMean.print3,2;//Step3:ComputePredictedMeanandCovariance//Welp,wealreadysolvedthemean)]TJ/F29 7.9701 Tf 12.3613 0 Td[(let'sdothecovariancenowforinti=0;i<=2L;i++fMatrixtempP=UpChi.geti.minusXhatMean;MatrixtempPw=tempP.timeswc.geti;tempP=tempPw.timestempP.transpose;UpPx=UpPx.plustempP;g//NewSteps!//Step4:ComputeMeasurements!andYmean!MatrixYhatMean=newMatrix3,1,0.0;forinti=0;i<=2L;i++fMatrixChiOne=Chi.geti;MatrixChiyminus=newMatrix3,1,0.0;DoubleXhat=UpChi.geti.get0,0;DoubleYhat=UpChi.geti.get1,0;DoubleZhat=UpChi.geti.get2,0;DoubleXerr=ChiOne.get6,0;DoubleYerr=ChiOne.get7,0;DoubleZerr=ChiOne.get8,0; 219

PAGE 232

AppendixAContinued Xhat=Xhat+Xerr;Yhat=Yhat+Yerr;Zhat=Zhat+Zerr;Chiyminus.set0,0,Xhat;Chiyminus.set1,0,Yhat;Chiyminus.set2,0,Zhat;UpY.addChiyminus;YhatMean=YhatMean.plusChiyminus.timeswm.geti;g////Welp,wealreadysolvedthemean)]TJ/F29 7.9701 Tf 12.3613 0 Td[(let'sdothecovariances//now//System.out.printlnXHatMeanandYHatMean=;//XhatMean.print3,2;//YhatMean.print3,2;forinti=0;i<=2L;i++fMatrixtempPx=UpChi.geti.minusXhatMean;MatrixtempPy=UpY.geti.minusYhatMean;//System.out.printlnChiX)]TJ/F29 7.9701 Tf 11.3287 0 Td[(XhatMeanandChiY)]TJ/F29 7.9701 Tf 6.2945 0 Td[(YhatMean;//tempPx.print3,2;//tempPy.print3,2;MatrixtempPxw=tempPx.timeswc.geti;MatrixtempPyw=tempPy.timeswc.geti;tempPx=tempPxw.timestempPy.transpose;tempPy=tempPyw.timestempPy.transpose;UpPy=UpPy.plustempPy;UpPxy=UpPxy.plustempPx;g//Step6:ComputeCorrectionsandUpdate 220

PAGE 233

AppendixAContinued //ComputeKalmanGain!//System.out.printlnUpdatedPx;//UpPx.print5,2;//System.out.printlnUpdatedPy;//UpPy.print5,2;//System.out.printlnUpdatedPxy;//UpPxy.print5,2;K=UpPxy.timesUpPy.inverse;//System.out.printlnKalman;//K.print5,2;MatrixMea=newMatrix3,1,0.0;Mea.set0,0,dMeasure.get0;Mea.set1,0,dMeasure.get1;Mea.set2,0,dMeasure.get2;MatrixOut=K.timesMea.minusYhatMean;Out=Out.plusXhatMean;//System.out.printlnOut:;//Out.print3,2;MatrixPx=UpPx.minusK.timesUpPy.timesK.transpose;//UpdateStuff!//PushthePtothecontrollerMatrixOutP=P.copy;OutP.setMatrix0,2,0,2,Px;X.setMatrix0,2,0,0,Out;Residual=XhatMean.minusOut;cController.inputStateOutP,Residual;//cController.setLL;cController.startProcess;while!cController.finishedProcessftryf 221

PAGE 234

AppendixAContinued Thread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;gg//System.out.printlnPostGreeks:+cController.getAlpha+,//+cController.getBeta;dGreek.set0,cController.getAlpha;dGreek.set1,cController.getBeta;dGreek.set2,cController.getKappa;P=cController.getP;//System.out.printlnPispostProcess:;//P.print3,2;StepDone=true;ggpublicVectorgetOutputfifProcessDonefreturnOutput;greturnnull;gpublicVectorgetResidualsfifProcessDonefreturnResid;greturnnull;g 222

PAGE 235

AppendixAContinued g testsystemCompassEntry.javapackagetestsystem;publicclassCompassEntryfDoubleTheta;DoubledTS;publicCompassEntryDoubleHeading,DoubleTimestampf//MakeHeadingintoRads)]TJ/F29 7.9701 Tf 11.7263 0 Td[(ThetaisstoredasRadsTheta=HeadingMath.PI/180;dTS=Timestamp;gpublicCompassEntryDoubleHeading,DoubleTimestamp,booleanRadsf//CheckifRads,thensetappropriatelydTS=Timestamp;ifRadsTheta=Heading;elseTheta=HeadingMath.PI/180;gpublicDoubleGetDegfreturnTheta180/Math.PI;gpublicDoubleGetRadfreturnTheta;g 223

PAGE 236

AppendixAContinued publicvoidSetDegDoubleHeadingfTheta=HeadingMath.PI/180;gpublicvoidSetRadDoubleHeadingfTheta=Heading;gpublicDoubleGetTimestepfreturndTS;gpublicvoidSetTimestepDoubleTimestampfdTS=Timestamp;gpublicCompassEntryCopyCompassEntryfCompassEntryretval=newCompassEntryTheta180/Math.PI,dTS;returnretval;gg 224

PAGE 237

AppendixAContinued Controllers.javapackagetestsystem;publicenumControllersfT,F,D,ET,ST,EF,SF,ED,SDg 225

PAGE 238

AppendixAContinued Data.javapackagetestsystem;importjava.io.;importjava.util.Scanner;importjava.util.Vector;importjavax.vecmath.Matrix3d;importjavax.vecmath.Matrix4d;importjavax.vecmath.Point3d;importjavax.vecmath.Vector3d;//Aclassforencompassingalldatastoragefromthefile.publicclassDatafprivateVectorIMUData=newVector;privateVectorGPSData=newVector;privateVectorOdoData=newVector;privateVectorCompassData=newVector;privateintEntryNum=0;publicDataStringdataFileNamefifdataFileName!=nullfEntryNum=0;tryf//Openthefilethatisthefirst//commandlineparameterSystem.out.printlnAttempting to open +dataFileName;FileReaderfstream=newFileReaderdataFileName;//ConvertourinputstreamtoaDataInputStream 226

PAGE 239

AppendixAContinued BufferedReaderin=newBufferedReaderfstream;System.out.printlnFile Open!+dataFileName;//Continuetoreadlineswhile//therearestillsomelefttoread//Blowpasttheheaderin.readLine;Stringline=;Doubletimestamp=0.0;Doubleelapsedtime=0.0;VectorIMUAccel=newVector;VectorIMURAccel=newVector;VectorIMURate=newVector;DoubleX,Y,Theta;DoubleHeading;DoubleGPSNorthY,GPSEastX;whileline=in.readLine!=nullf//pulldownaline,thencrushit//System.out.printlnReadingaLine!;Scanners=newScannerline;//System.out.printlnline;timestamp=Double.valueOfs.next;elapsedtime=Double.valueOfs.next;IMUAccel.clear;IMUAccel.addDouble.valueOfs.next;IMUAccel.addDouble.valueOfs.next;IMUAccel.addDouble.valueOfs.next;IMURAccel.clear;//Thesearen'treallyangles.Fakeit!IMURAccel.addDouble.valueOfs.next;IMURAccel.addDouble.valueOfs.next;IMURAccel.add0.0;Heading=Double.valueOfs.next;IMURate.clear;IMURate.addDouble.valueOfs.next; 227

PAGE 240

AppendixAContinued IMURate.addDouble.valueOfs.next;IMURate.addDouble.valueOfs.next;X=Double.valueOfs.next;Y=Double.valueOfs.next;Theta=Double.valueOfs.next;GPSNorthY=Double.valueOfs.next;GPSEastX=Double.valueOfs.next;//System.out.printlnX+,+Y+,+Theta;IMUData.addnewIMUEntryIMUAccel,IMURAccel,timestamp;GPSData.addnewGPSEntryGPSEastX,GPSNorthY,timestamp;OdoData.addnewOdoEntryX,Y,Theta,timestamp;CompassData.addnewCompassEntryHeading,timestamp;EntryNum++;gin.close;gcatchExceptionefSystem.err.printlnFile input error;ggelseSystem.out.printlnInvalid parameters;gpublicIMUEntryGetIMUEntryintiEntryfifIMUData!=nullreturnIMUData.elementAtiEntry;elseSystem.out.printlnIMUDaaaata is empty?;returnnull;gpublicVectorGetIMUVelocityintiEntry,VectorDeltaHf//IMUData//IMUTime//IMUX+=backwards//IMUY+=movesleftviewedfrombehind 228

PAGE 241

AppendixAContinued //IMUZ+=movesdown//IMUXRot=RHRaroundbackwardsfacingaxisrobotrotates//counterclockwiserollasviewedfrombehind//IMUYRot=RHRaroundleftaxisPitchtowardsground=higher//number//IMUZRot=RHRarounddownaxisRobotturningright=highernumberVectortempAcc=IMUData.getiEntry.getIMUAcceleration;DoubleTS=IMUData.getiEntry.getIMUTimestamp;//System.out.printlnIMUAccels:+tempAcc.get0+,+tempAcc.get1+,+tempAcc.get2;Matrix4dmRotX=newMatrix4d;mRotX.rotXDeltaH.get0;Matrix4dmRotY=newMatrix4d;mRotY.rotYDeltaH.get1;Matrix4dmRotZ=newMatrix4d;mRotZ.rotZ)]TJ/F16 7.9701 Tf 7.4782 0 Td[(DeltaH.get2;mRotX.mulmRotY;mRotX.mulmRotZ;Point3dAccel=newPoint3d;Accel.settempAcc.get0,tempAcc.get1,tempAcc.get2;mRotX.transformAccel;tempAcc.set0,Accel.x;tempAcc.set1,Accel.y;tempAcc.set2,Accel.z;//System.out.printlnIMUAfterRotationAccels:+tempAcc.get0+,+tempAcc.get1+,+tempAcc.get2;returntempAcc;g 229

PAGE 242

AppendixAContinued publicVectorGetIMURotVelocityintiEntryfVectortempRot=IMUData.getiEntry.getIMURotAccel;DoubleTS=IMUData.getiEntry.getIMUTimestamp;System.out.printlnDon't call this! Broken!;tempRot.set0,tempRot.get0TS;tempRot.set0,tempRot.get1TS;tempRot.set0,tempRot.get2TS;returntempRot;gpublicOdoEntryGetOdoEntryintiEntryfifOdoData!=nullreturnOdoData.elementAtiEntry;elsereturnnull;gpublicGPSEntryGetGPSEntryintiEntryfifGPSData!=nullreturnGPSData.elementAtiEntry;elsereturnnull;gpublicCompassEntryGetCompassEntryintiEntryfifCompassData!=nullreturnCompassData.elementAtiEntry;elsereturnnull;gpublicintgetLengthfreturnEntryNum;gg 230

PAGE 243

AppendixAContinued Filters.javapackagetestsystem;publicenumFiltersfEKF,SPKF,PFg 231

PAGE 244

AppendixAContinued GPSEntry.javapackagetestsystem;//GPSEntry/@authorjeffreykramer/publicclassGPSEntryf/DOCUMENTME!/Doubletimestamp;DoubleNorthY;DoubleEastX;//GPSentriesareinUTMsetinZone17intheNorthernHemispheremeters/@paramiEastX@paramiNorthY@paramtimestamp/publicGPSEntryDoubleiEastX,DoubleiNorthY,Doubletimestampfthis.timestamp=timestamp;NorthY=iNorthY;EastX=iEastX;g/@paramiNorthY/publicvoidsetNorthYDoubleiNorthYfNorthY=iNorthY;g 232

PAGE 245

AppendixAContinued /@return/publicDoublegetNorthYfreturnNorthY;g/@paramtimestamp/publicvoidsetGPSTimestampDoubletimestampfthis.timestamp=timestamp;g/@return/publicDoublegetGPSTimestampfreturntimestamp;g/@paramiEastX/publicvoidsetEastXDoubleiEastXfEastX=iEastX;g/@return/publicDoublegetEastXfreturnEastX;g 233

PAGE 246

AppendixAContinued /DOCUMENTME!@returnDOCUMENTME!/publicStringtoStringfStringBuffersb=newStringBuffer;sb.appendTimestamp: ;sb.appendtimestamp.toString;sb.append;sb.append Location: ;sb.appendEastX.toString+ +NorthY.toString;returnsb.toString;g//privateGMatrixgetECEFf////AdaptedfromLLA2ECEF.mbyMichaelKleder////WGS84ellipsoidconstants//Doublea=6378137;////Doublee=0.081819190842622;////OtherVariables//DoubleN,x,y,z,tLat,tLon,alt;//GMatrixmOutput=newGMatrix1,3;////alt=0;//Altitude=0?////Calculate!////tLat=NorthX;//tLon=EastY;////IntermediateCalcprimeverticalradiusofcurvature//N=a/Math.sqrt1)]TJ/F29 7.9701 Tf 11.8312 0 Td[(Math.EMath.EMath.sintLat;//////Results//x=N+altMath.costLatMath.costLon; 234

PAGE 247

AppendixAContinued //y=N+altMath.costLatMath.sintLon;//z=1)]TJ/F29 7.9701 Tf 11.8312 0 Td[(Math.expMath.EN+altMath.sintLat;//////PutitinaMatrix//mOutput.setElement0,0,x;//mOutput.setElement0,1,y;//mOutput.setElement0,2,z;////returnmOutput;//gg 235

PAGE 248

AppendixAContinued IMUEntry.javapackagetestsystem;importjava.util.Vector;//IMUEntrypublicclassIMUEntryfVectoraccel=newVector;Vectorraccel=newVector;Doubletimestamp;publicIMUEntryVectord accel,Vectord raccel,Doubletimestampfthis.accel=d accel;this.raccel=d raccel;this.timestamp=timestamp;gpublicvoidsetIMUAccelerationVectord accelfthis.accel=d accel;gpublicVectorgetIMUAccelerationfreturnaccel;gpublicvoidsetIMURotAccelVectord raccelfthis.raccel=d raccel;gpublicVectorgetIMURotAccelfreturnraccel;g 236

PAGE 249

AppendixAContinued publicvoidsetIMUTimestampDoubletimestampfthis.timestamp=timestamp;gpublicDoublegetIMUTimestampfreturntimestamp;gpublicIMUEntrycopyIMUEntryfIMUEntryretval=newIMUEntryaccel,raccel,timestamp;returnretval;gpublicStringtoStringfStringretval= Acceleration Vector:nn [+accel.get0+, +accel.get1+, +accel.get2+]nn;retval=retval+ Rotational Acceleration Vector:nn [+raccel.get0+, +raccel.get1+, +raccel.get2+]nn;retval=retval+ Timestamp = +timestamp;returnretval;gg 237

PAGE 250

AppendixAContinued OdoEntry.javapackagetestsystem;publicclassOdoEntryfdoubleX,Y,theta;doubletimestamp;publicOdoEntrydoubled X,doubled Y,doubled Theta,doubletimestampfthis.X=d X;this.Y=d Y;this.theta=d Theta;this.timestamp=timestamp;gpublicvoidsetOdoXdoubled Xfthis.X=d X;gpublicvoidsetOdoYdoubled Yfthis.Y=d Y;gpublicvoidsetOdoThetadoubled Thetafthis.theta=d Theta;gpublicdoublegetOdoXfreturnX;gpublicdoublegetOdoYfreturnY;gpublicdoublegetOdoThetafreturntheta;g 238

PAGE 251

AppendixAContinued publicvoidsetOdoTimestampdoubletimestampfthis.timestamp=timestamp;gpublicdoublegetOdoTimestampfreturntimestamp;gpublicOdoEntryCopyOdoEntryfOdoEntryretval=newOdoEntryX,Y,theta,timestamp;returnretval;gg 239

PAGE 252

AppendixAContinued Test.javapackagetestsystem;importjava.io.BufferedReader;importjava.io.FileNotFoundException;importjava.io.FileReader;importjava.io.IOException;importjava.util.Scanner;importjava.util.Vector;importfilters.;importcontrollers.;publicclassTestfprivateVectorFilterList=newVector;privatebooleanDoneCrunching=false;publicTestStringFilename,DataDataRunfifFilename!=nullf//Openthefilethatisthefirst//commandlineparameterFileReaderfstream;tryffstream=newFileReaderFilename;//ConvertourinputstreamtoaDataInputStreamBufferedReaderin=newBufferedReaderfstream;System.out.printlnReading a Test file! +Filename;Stringline;Stringfilter;Stringcontroller;Stringoption;Doubleoptionnum;DoubledQ;DoubledR; 240

PAGE 253

AppendixAContinued DoubleAlpha,Beta,Kappa;StringFuzzyFileName;intNumFilters;intNumParticles;whileline=in.readLine!=nullf//pulldownaline,thencrushit//System.out.printlnline;Scanners=newScannerline;filter=s.next;//System.out.printlnfilter;iffilter.contains#f//Skipping)]TJ/F29 7.9701 Tf 11.4415 0 Td[(commentedout.System.out.printlnSkipped )]TJET1 0 0 1 305.1029 461.0103 cmq[]0 d0 J0.3985 w0.1992 0 m0.1992 1.076 lSQ1 0 0 1 0.3985 0 cmq[]0 d0 J0.3985 w0 0.1992 m2.3911 0.1992 lSQ1 0 0 1 2.391 0 cmq[]0 d0 J0.3985 w0.1992 0 m0.1992 1.076 lSQ1 0 0 1 -307.8924 -461.0103 cmBT/F16 7.9701 Tf 309.2562 461.0103 Td[(Commented Out )]TJET1 0 0 1 381.6167 461.0103 cmq[]0 d0 J0.3985 w0.1992 0 m0.1992 1.076 lSQ1 0 0 1 0.3985 0 cmq[]0 d0 J0.3985 w0 0.1992 m2.3911 0.1992 lSQ1 0 0 1 2.3911 0 cmq[]0 d0 J0.3985 w0.1992 0 m0.1992 1.076 lSQ1 0 0 1 -384.4063 -461.0103 cmBT/F16 7.9701 Tf 385.9844 461.0103 Td[(+line;gelseiffilter.containsEKFfcontroller=s.next;ifcontroller.contains)]TJ/F16 7.9701 Tf 5.9413 0 Td[(Tf//Okay,we'relookingatatraditionalEKFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;FilterList.addnewEKFnewNoControlEKF,DataRun,dQ,dR;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!; 241

PAGE 254

AppendixAContinued break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 6.1604 0 Td[(Ff//Okay,we'relookingatafuzzyEKFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(If//Okay,we'recorrectsofarFuzzyFileName=s.next;FilterList.addnewEKFnewFuzzyEKFFuzzyFileName,DataRun,dQ,dR;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(DfSystem.out.printlnSkipping )]TJ/F16 7.9701 Tf 6.1624 0 Td[(Ds, sorry...; 242

PAGE 255

AppendixAContinued gelseifcontroller.contains)]TJ/F16 7.9701 Tf 4.8932 0 Td[(MHf//Okay,we'relookingataMultihypothesisoneNumFilters=s.nextInt;VectortempQ=newVector;VectortempR=newVector;forinti=0;i
PAGE 256

AppendixAContinued forinti=0;i
PAGE 257

AppendixAContinued gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.6317 0 Td[(TOf//Okay,we'relookingatatraditionalSPKFw///optionsoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;Alpha=s.nextDouble;Beta=s.nextDouble;Kappa=s.nextDouble;FilterList.addnewSPKFnewNoControlSPKF9,DataRun,dQ,dR,Alpha,Beta,Kappa;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;g 245

PAGE 258

AppendixAContinued gelseifcontroller.contains)]TJ/F16 7.9701 Tf 6.1604 0 Td[(Ff//Okay,we'relookingatafuzzySPKFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(If//Okay,we'recorrectsofarFuzzyFileName=s.next;FilterList.addnewSPKFnewFuzzySPKFFuzzyFileName,9,DataRun,dQ,dR;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.7779 0 Td[(FOf//Okay,we'relookingatafuzzySPKFwithOptions!option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf 246

PAGE 259

AppendixAContinued //Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(If//Okay,we'recorrectsofarFuzzyFileName=s.next;Alpha=s.nextDouble;Beta=s.nextDouble;Kappa=s.nextDouble;FilterList.addnewSPKFnewFuzzySPKFFuzzyFileName,9,DataRun,dQ,dR,Alpha,Beta,Kappa;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.7779 0 Td[(DFf//Okay,we'relookingatadoublefuzzySPKFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofar 247

PAGE 260

AppendixAContinued optionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(If//Okay,we'recorrectsofarFuzzyFileName=s.next;FilterList.addnewSPKFnewFuzzySPKFFuzzyFileName,dfkfis.txt,9,DataRun,dQ,dR;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.5866 0 Td[(DFOf//Okay,we'relookingatadoublefuzzySPKFwith//Options!option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next; 248

PAGE 261

AppendixAContinued ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(If//Okay,we'recorrectsofarFuzzyFileName=s.next;Alpha=s.nextDouble;Beta=s.nextDouble;Kappa=s.nextDouble;FilterList.addnewSPKFnewFuzzySPKFFuzzyFileName,dfkfis.txt,9,DataRun,dQ,dR,Alpha,Beta,Kappa;//Filtercreated!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(DfSystem.out.printlnSkipping )]TJ/F16 7.9701 Tf 6.1624 0 Td[(Ds, sorry...;gelseifcontroller.contains)]TJ/F16 7.9701 Tf 4.8932 0 Td[(MHf//Okay,we'relookingataMultihypothesisoneNumFilters=s.nextInt;VectortempQ=newVector;VectortempR=newVector;forinti=0;i
PAGE 262

AppendixAContinued option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofartempQ.adds.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofartempR.adds.nextDouble;gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.9412 0 Td[(TfMHSPKFtempMH=newMHSPKFnewNoControlSPKF9,DataRun,tempQ.get0,tempR.get0;forinti=1;i
PAGE 263

AppendixAContinued break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseiffilter.containsPFfcontroller=s.next;ifcontroller.contains)]TJ/F16 7.9701 Tf 5.9266 0 Td[(ETf//ThisisatradEKFPFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newNoControlEKF,2,dQ,dR;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!; 251

PAGE 264

AppendixAContinued break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 6.0728 0 Td[(STf//ThisisatradSPKFPFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newNoControlSPKF9,3,dQ,dR;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 6.0728 0 Td[(EFf//ThisisaFuzzyEKFPF 252

PAGE 265

AppendixAContinued option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(IfFuzzyFileName=s.next;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newFuzzyEKFFuzzyFileName,2,dQ,dR;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;g 253

PAGE 266

AppendixAContinued gelseifcontroller.contains)]TJ/F16 7.9701 Tf 6.2189 0 Td[(SFf//ThisisaFuzzySPKFPFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(IfFuzzyFileName=s.next;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newFuzzySPKFFuzzyFileName,9,3,dQ,dR;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break; 254

PAGE 267

AppendixAContinued ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.9174 0 Td[(SFOf//ThisisaFuzzySPKFPFwithOptionsoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(IfFuzzyFileName=s.next;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;Alpha=s.nextDouble;Beta=s.nextDouble;Kappa=s.nextDouble;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newFuzzySPKFFuzzyFileName,9,3,dQ,dR,Alpha,Beta,Kappa;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!; 255

PAGE 268

AppendixAContinued break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.9174 0 Td[(SDFf//ThisisaDoubleFuzzySPKFPFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(IfFuzzyFileName=s.next;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newFuzzySPKFFuzzyFileName,dfkfis.txt,9,3,dQ,dR;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;g 256

PAGE 269

AppendixAContinued gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.7364 0 Td[(SDFOf//ThisisaDoubleFuzzySPKFPFwithoptionsoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 7.0491 0 Td[(IfFuzzyFileName=s.next;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;Alpha=s.nextDouble;Beta=s.nextDouble;Kappa=s.nextDouble;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newFuzzySPKFFuzzyFileName,dfkfis.txt,9,3,dQ,dR,Alpha,Beta,Kappa;//FilterAdded!gelsefSystem.out.printlnMisformed Controller!; 257

PAGE 270

AppendixAContinued break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.8077 0 Td[(STOf//ThisisatradSPKFPFwithoptionsoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;Alpha=s.nextDouble;Beta=s.nextDouble;Kappa=s.nextDouble;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newNoControlSPKF9,3,dQ,dR,Alpha,Beta,Kappa;//FilterAdded! 258

PAGE 271

AppendixAContinued gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.6317 0 Td[(EDfSystem.out.printlnSkipping )]TJ/F16 7.9701 Tf 6.1624 0 Td[(Ds, sorry...;gelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.7778 0 Td[(SDfSystem.out.printlnSkipping )]TJ/F16 7.9701 Tf 6.1624 0 Td[(Ds, sorry...;gelseifcontroller.contains)]TJ/F16 7.9701 Tf 5.9412 0 Td[(Tf//ThisisatraditionalPFoption=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.4989 0 Td[(Qf//Okay,we'recorrectsofaroptionnum=s.nextDouble;dQ=optionnum;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 5.7181 0 Td[(Rf//Okay,we'recorrectsofardR=s.nextDouble;option=s.next;ifoption.contains)]TJ/F16 7.9701 Tf 6.1605 0 Td[(Pf//Okay,we'recorrectsofarNumParticles=s.nextInt;FilterList.addnewPFnewNoControlPF,DataRun,NumParticles,newNoControlSPKF9,1,dQ,dR; 259

PAGE 272

AppendixAContinued //FilterAdded!gelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnMisformed Controller!;break;ggelsefSystem.out.printlnWrong Filter Descript!;break;gggcatchFileNotFoundExceptionefSystem.out.printlnCouldn't find Test File!;e.printStackTrace;gcatchIOExceptionefSystem.out.printlnOh man, test file explosion!;e.printStackTrace;ggelsefSystem.out.printlnFile Name is bad for the Test File; 260

PAGE 273

AppendixAContinued gDoneCrunching=true;gpublicVectorGetFilterListfifDoneCrunchingf//Okay,takeit.returnFilterList;gelseSystem.out.printlnNot Done Crunching!;returnnull;gpublicbooleanFinishedParsingfreturnDoneCrunching;gg 261

PAGE 274

AppendixAContinued TestSystem.javapackagetestsystem;importjava.util.Date;importjava.util.Vector;importfilters.Filter;importtestsystem.Data;publicclassTestSystemf/@paramargs/publicstaticvoidmainString[]argsfStringDataFileName=args[0];StringTestFileName=args[1];//LoadtheDataFileandParseSystem.out.printlnLoading Data File;DataDataRun=newDataDataFileName;System.out.printlnFinished Parsing Data File;//LoadtheTestFileandParseSystem.out.printlnLoading Test File;TestTestList=newTestTestFileName,DataRun;while!TestList.FinishedParsingftryfThread.sleep10;gcatchInterruptedExceptionefe.printStackTrace;gg 262

PAGE 275

AppendixAContinued System.out.printlnFinished Parsing Test File;VectorFilters=TestList.GetFilterList;DateCurDate=newDate;StringDateString=CurDate.toString;forinti=0;i
PAGE 276

AppendixBRobotSimulationCode runbuilderBuildRun.javapackagerunbuilder;importjava.io.;importjava.util.Random;importjava.util.Vector;publicclassBuildRunf/@paramargs///Args://0)]TJ/F29 7.9701 Tf 11.7032 0 Td[(OutputFileNamewilladd.txt//1)]TJ/F29 7.9701 Tf 11.7421 0 Td[(MicrosecondsofrundurationORInputFileNameORRandomwithoutquotes//2)]TJ/F29 7.9701 Tf 11.7422 0 Td[(0or1GPSmalfunctionwilladdrandomGPSmalfunction//3)]TJ/F29 7.9701 Tf 11.7422 0 Td[(0or1IMUmalfunctionwilladdrandomIMUmalfunction//4)]TJ/F29 7.9701 Tf 11.7422 0 Td[(0or1Compassmalfunctionwilladdrandomcompassmalfunction//5)]TJ/F29 7.9701 Tf 11.7422 0 Td[(0or1OdoMalfunctionwilladdrandomOdoMalfunctionpublicstaticvoidmainString[]argsfbooleanGPSMal=false;booleanIMUMal=false;booleanCompassMal=false;booleanOdoMal=false;booleanGPSBridge=false;booleanSimple=true;booleanRandom=false;IntegerRunDuration;VectorPoints=newVector;StringCheckDown=args[1];ifCheckDown.contains.csvfRunDuration=0; 264

PAGE 277

AppendixBContinued Simple=false;Random=false;gelseifCheckDown.equalsIgnoreCaseRandomfRunDuration=0;Simple=false;Random=true;gelsefSimple=true;Random=false;RunDuration=Integer.valueOfCheckDown;gStringDataOut=args[0];//System.out.printlnargs[0];//System.out.printlnargs[1];//System.out.printlnargs[2];//System.out.printlnargs[3];//System.out.printlnargs[4];//System.out.printlnargs[5];//System.out.printlnargs[6];ifargs[2].equals1GPSMal=true;ifargs[3].equals1GPSBridge=true;GPSBridge=true;ifargs[4].equals1IMUMal=true;ifargs[5].equals1CompassMal=true;ifargs[6].equals1OdoMal=true;if!SimpleifRandomfRandomr=newRandom;//Generatearandomnumberofpointsfrom15)]TJ/F29 7.9701 Tf 6.7842 0 Td[(20 265

PAGE 278

AppendixBContinued //intj=r.nextInt6+15;intj=20;System.out.printlnNumber of Random Points: +j;forinti=0;i
PAGE 279

AppendixBContinued RunMakerrun;ifSimplerun=newRunMakerGPSMal,IMUMal,CompassMal,OdoMal,GPSBridge,RunDuration;elserun=newRunMakerGPSMal,IMUMal,CompassMal,OdoMal,GPSBridge,Points;tryfBufferedWriterdout=newBufferedWriternewFileWriterDataOut;BufferedWritercout=newBufferedWriternewFileWriterControlOut;dout.write#timestamp elapsedtime IMUAccelX IMUAccelY IMUAccelZ HeadingX HeadingY HeadingActual unused unused unused XOdo YOdo Theta GPSY GPSX;dout.newLine;//cout.writetimestamp,elapsedtime,ActualX,ActualY,ActualHeading;cout.write#timestamp elapsedtime ActualX ActualY ActualHeading;cout.newLine;while!run.Finishedfrun.step;ifrun.updatedfdout.writerun.getD;dout.newLine;cout.writerun.getC;cout.newLine;ggdout.close;cout.close;gcatchIOExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;ggg 267

PAGE 280

AppendixBContinued RunMaker.javapackagerunbuilder;importjava.util.;publicclassRunMakerfprivatebooleanGPSMal;privatebooleanIMUMal;privatebooleanCompassMal;privatebooleanOdoMal;privatebooleanGPSBrid;privateIntegerRunLength;privateIntegerCurrentRun;privatebooleanStepLock,OutputLock,DLock,CLock;privatebooleanFinished,Updated;//privatestaticDoubleTimestepSize=0.0056;privateStringsDataSpace= ;privatestaticDoubleTimestepSize=1.0;//Inms//0.0001;privatestaticintMalfunProb=100000;//Asin1/XndprivatestaticDoubleGPSErr=2.5;privatestaticDoubleIMUErr=0.1176; 268

PAGE 281

AppendixBContinued privatestaticDoubleCompassErr=0.8Math.PI/180;//RadsnowprivatestaticDoubleOdoErr=0.0001;privatestaticIntegerIMURate=10;privatestaticIntegerGPSRate=40;privatestaticIntegerCompassRate=125;privatestaticDoubleRobotFrameVel=0.002;//0.0008;privatestaticDoubleRobotFrameRot=0.0001;//inRadsprivatestaticDoubleBridgeEast=70.0;//EastendofthebridgeprivatestaticDoubleBridgeWest=80.0;//WestendofthebridgeprivateDoubleGPSX,GPSY,IMUAccelX,IMUAccelY,IMUAccelZ,HeadingX,HeadingY,Heading,XOdo,YOdo,Theta,OldFrameVel,OldFrameRot,OldActualX,OldActualY,OldActualHeading,ActualSpeed;privateDoubleActualX,ActualY,ActualHeading;privateRandomr=newRandom;privateIntegerStepCalc;privateBooleanSimple,Stop,Go,Start,Aim;privateVectorPoints;privatePointNew;privateIntegerPointCounter,NumPoints; 269

PAGE 282

AppendixBContinued publicRunMakerbooleanGPSMalfun,booleanIMUMalfun,booleanCompassMalfun,booleanOdoMalfun,booleanGPSBridge,IntegerRunDurationfGPSMal=GPSMalfun;IMUMal=IMUMalfun;CompassMal=CompassMalfun;OdoMal=OdoMalfun;GPSBrid=GPSBridge;RunLength=RunDuration;CurrentRun=0;GPSX=100.0;GPSY=100.0;IMUAccelX=0.0;IMUAccelY=0.0;IMUAccelZ=0.0;HeadingX=0.0;HeadingY=0.0;Heading=Math.PI/2;//FacingNXOdo=0.0;YOdo=0.0;Theta=0.0;//OldFrameVel=0.0;//OldFrameRot=0.0;OldActualX=ActualX=100.0;OldActualY=ActualY=100.0;OldActualHeading=ActualHeading=Math.PI/2;//FacingNOutputLock=false;DLock=false;CLock=false;Finished=false;Updated=false;StepCalc=0;Simple=true;gpublicRunMakerbooleanGPSMalfun,booleanIMUMalfun,booleanCompassMalfun,booleanOdoMalfun,booleanGPSBridge,VectorFreshPointsf 270

PAGE 283

AppendixBContinued GPSMal=GPSMalfun;IMUMal=IMUMalfun;CompassMal=CompassMalfun;OdoMal=OdoMalfun;GPSBrid=GPSBridge;//RunLength=RunDuration;CurrentRun=0;GPSX=500.0;GPSY=500.0;IMUAccelX=0.0;IMUAccelY=0.0;IMUAccelZ=0.0;HeadingX=0.0;HeadingY=0.0;Heading=Math.PI/2;//FacingNXOdo=0.0;YOdo=0.0;Theta=0.0;//OldFrameVel=0.0;//OldFrameRot=0.0;OldActualX=ActualX=500.0;OldActualY=ActualY=500.0;OldActualHeading=ActualHeading=Math.PI/2;//FacingNActualSpeed=0.0;OutputLock=false;DLock=false;CLock=false;Finished=false;Updated=false;StepCalc=0;Simple=false;Points=FreshPoints;PointCounter=0;NumPoints=Points.size;Start=true;Aim=false; 271

PAGE 284

AppendixBContinued Stop=false;Go=false;gpublicvoidstepfifDLock==false&&CLock==falsefStepLock=true;CurrentRun+=TimestepSize;StepCalc+=TimestepSize;ifSimplef//ChecktypeofcalculationifCurrentRun=Math.PIfActualHeading)]TJ/F16 7.9701 Tf 6.2072 0 Td[(=2Math.PI;gActualX+=RFrameVelMath.cosActualHeading;ActualY+=RFrameVelMath.sinActualHeading;gelsef//We'redone!Finished=true;StepLock=false;ggelseifPointCounter
PAGE 285

AppendixBContinued DoubleTurningSpeed=RobotFrameRot10;//Old=newPointOldActualX,OldActualY;New=newPointActualX,ActualY;ifStop==true&&ActualSpeed>0.0f//StopCaseActualSpeed=ActualSpeed/2.0;ifActualSpeed<0.0001fActualSpeed=0.0;Stop=false;Start=true;ggelseifGo==truefifActualSpeed=TurningSpeedActualHeading)]TJ/F16 7.9701 Tf 6.2072 0 Td[(=TurningSpeed;elseActualHeading=CurrentHeading; 273

PAGE 286

AppendixBContinued gelsefifCurrentHeading)]TJ/F16 7.9701 Tf 11.7612 0 Td[(ActualHeading>=TurningSpeedActualHeading+=TurningSpeed;elseActualHeading=CurrentHeading;gifActualHeading>=Math.PIActualHeading)]TJ/F16 7.9701 Tf 6.2072 0 Td[(=2Math.PI;ifActualHeading<)]TJ/F16 7.9701 Tf 6.4458 0 Td[(Math.PIActualHeading+=2Math.PI;g//System.out.printlnHeadingDiff:+CurrentHeading)]TJ/F29 7.9701 Tf 11.7607 0 Td[(ActualHeading;gelsef//Startmustbetrueif!StartSystem.out.printlnError! How'd we end up here?;ifStartfSystem.out.printlnAiming at new point!;Start=false;Aim=true;gg//MadeourSpeedChoices,etc)]TJ/F29 7.9701 Tf 11.7669 0 Td[(updateActuals.ActualX+=ActualSpeedMath.cosActualHeading;ActualY+=ActualSpeedMath.sinActualHeading;//System.out.printlnState:Start,Aim,Go,Stop)]TJ/F29 7.9701 Tf 10.9426 0 Td[(+Start+,+Aim+,+Go+,+Stop;//System.out.printlnSpeed,Heading,XandY:+ActualSpeed+//,+ActualHeading+,+ActualX+,+ActualY;gelsef//We'redone! 274

PAGE 287

AppendixBContinued Finished=true;StepLock=false;gifCurrentRun%IMURate==0&&!Finishedf//IMUUpdate!DoubleXAcc=ActualX)]TJ/F16 7.9701 Tf 11.5729 0 Td[(OldActualX/Math.powdoubleIMURate/1000,2;DoubleYAcc=ActualY)]TJ/F16 7.9701 Tf 11.5729 0 Td[(OldActualY/Math.powdoubleIMURate/1000,2;XAcc+=r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(2+1r.nextDoubleIMUErr;YAcc+=r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(2+1r.nextDoubleIMUErr;//CheckformalfunctionifIMUMalf//PossibletoMalfunctionifr.nextIntMalfunProb==1f//Ohgreat,nowyou'vedoneit!System.out.printlnIMU ERROR;XAcc=r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(2+1r.nextDouble10;YAcc=r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(2+1r.nextDouble10;ggIMUAccelX=XAcc;IMUAccelY=YAcc;IMUAccelZ=0.0;Updated=true;gifCurrentRun%GPSRate==0&&!Finishedf//GPSUpdate!ifr.nextBooleanGPSX=ActualX+r.nextDoubleGPSErr;elseGPSX=ActualX)]TJ/F16 7.9701 Tf 11.8312 0 Td[(r.nextDoubleGPSErr;//System.out.printlnActualXvs.GPSX;//System.out.printlnActualX; 275

PAGE 288

AppendixBContinued //System.out.printlnGPSX;//System.out.printlnGPSX)]TJ/F29 7.9701 Tf 11.7416 0 Td[(ActualX;ifr.nextBooleanGPSY=ActualY+r.nextDoubleGPSErr;elseGPSY=ActualY)]TJ/F16 7.9701 Tf 11.8312 0 Td[(r.nextDoubleGPSErr;//System.out.printlnActualYvs.GPSY;//System.out.printlnActualY;//System.out.printlnGPSY;//System.out.printlnGPSY)]TJ/F29 7.9701 Tf 11.7964 0 Td[(ActualY;//checkformalfunctionifGPSBridf//Wehaveabridge!Addmultipatherrorsifunderthe//bridgeifActualXBridgeWestf//Underbridge!//System.out.printlnUndertheBridge;DoubleBridgeDiff=BridgeEast)]TJ/F16 7.9701 Tf 11.6534 0 Td[(BridgeWest;ifr.nextBooleanGPSX=GPSX+r.nextDoubleBridgeDiff;elseGPSX=GPSX)]TJ/F16 7.9701 Tf 11.8312 0 Td[(r.nextDoubleBridgeDiff;ifr.nextBooleanGPSY=GPSY+r.nextDoubleBridgeDiff;elseGPSY=GPSY)]TJ/F16 7.9701 Tf 11.8312 0 Td[(r.nextDoubleBridgeDiff;ggifGPSMalf//Possible?ifr.nextIntMalfunProb==1f//Jeez...System.out.printlnGPS ERROR; 276

PAGE 289

AppendixBContinued GPSX=100+r.nextGaussian80;GPSY=100+r.nextGaussian80;ggUpdated=true;gifCurrentRun%CompassErr==0&&!Finishedf//CompassUpdate!Heading=ActualHeading+r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(2+1r.nextGaussianCompassErr;ifCompassMalf//Possible?ifr.nextIntMalfunProb==1f//OmgSystem.out.printlnCOMPASS ERROR;Heading=Math.PI/2;ggifHeading>=Math.PIfHeading)]TJ/F16 7.9701 Tf 6.2072 0 Td[(=2Math.PI;gUpdated=true;gXOdo+=ActualX)]TJ/F16 7.9701 Tf 11.573 0 Td[(OldActualX+r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(1OdoErrActualX)]TJ/F16 7.9701 Tf -346.289 -15.7774 Td[(OldActualX;//YOdo+=ActualY)]TJ/F16 7.9701 Tf 11.573 0 Td[(OldActualY+r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(1OdoErrActualY)]TJ/F16 7.9701 Tf -346.289 -15.7774 Td[(OldActualY;Theta+=ActualHeading)]TJ/F16 7.9701 Tf 11.7265 0 Td[(OldActualHeading+r.nextInt2)]TJ/F16 7.9701 Tf 7.128 0 Td[(2+10.001ActualHeading)]TJ/F16 7.9701 Tf 11.7264 0 Td[(OldActualHeading;//OldFrameVel=RFrameVel; 277

PAGE 290

AppendixBContinued //OldFrameRot=RFrameRot;OldActualX=ActualX;OldActualY=ActualY;OldActualHeading=ActualHeading;StepLock=false;gg//Otherwise,waitfortheLockstoclear!publicbooleanFinishedfifFinishedSystem.out.printlnTime Elapsed in Seconds: +CurrentRun/1000;returnFinished;gpublicbooleanupdatedfifUpdatedfUpdated=false;DLock=true;CLock=true;return!Updated;greturnUpdated;gpublicStringgetDf//GetaDataStringStringData=;//timestamp,elapsedtime,IMUAccelX,IMUAccelY,IMUAccelZ,HeadingX,//HeadingY,HeadingActual,unused,unused,unused,XOdo,YOdo,Theta,//GPSY,GPSX;Data=Data+doubleStepCalc/1000; 278

PAGE 291

AppendixBContinued Data=Data+sDataSpace;Data=Data+doubleCurrentRun/1000;Data=Data+sDataSpace;Data=Data+IMUAccelX;Data=Data+sDataSpace;Data=Data+IMUAccelY;Data=Data+sDataSpace;Data=Data+IMUAccelZ;Data=Data+sDataSpace;Data=Data+HeadingX;Data=Data+sDataSpace;Data=Data+HeadingY;Data=Data+sDataSpace;Data=Data+Heading;Data=Data+sDataSpace;Data=Data+0;Data=Data+sDataSpace;Data=Data+0;Data=Data+sDataSpace;Data=Data+0;Data=Data+sDataSpace;Data=Data+XOdo;Data=Data+sDataSpace;Data=Data+YOdo;Data=Data+sDataSpace;Data=Data+Theta;Data=Data+sDataSpace;Data=Data+GPSY;Data=Data+sDataSpace;Data=Data+GPSX;DLock=false;ifCLock==falseStepCalc=0;returnData;g 279

PAGE 292

AppendixBContinued publicStringgetCf//GetaControlStringStringControl=;//timestamp,elapsedtime,ActualX,ActualY,ActualHeading;Control=Control+doubleStepCalc/1000;Control=Control+sDataSpace;Control=Control+doubleCurrentRun/1000;Control=Control+sDataSpace;Control=Control+ActualX;Control=Control+sDataSpace;Control=Control+ActualY;Control=Control+sDataSpace;Control=Control+ActualHeading;CLock=false;ifDLock==falseStepCalc=0;returnControl;gg 280

PAGE 293

AppendixBContinued Point.javapackagerunbuilder;publicclassPointfprivateDoublex;//x)]TJ/F29 7.9701 Tf 6.8922 0 Td[(coordinateprivateDoubley;//y)]TJ/F29 7.9701 Tf 6.8922 0 Td[(coordinatepublicPointf//From0to1x=Math.random;y=Math.random;gpublicPointDoublex,Doubleyf//Pointcreatedfrom#sthis.x=x;this.y=y;g//ReturnspublicDoublegetXfreturnx;gpublicDoublegetYfreturny;gpublicDoublegetRfreturnMath.sqrtxx+yy;gpublicDoublegetThetafreturnMath.atan2y,x;g 281

PAGE 294

AppendixBContinued publicDoublegetThetaPointthatfDoubledx=that.x)]TJ/F31 7.9701 Tf 12.1126 0 Td[(this.x;Doubledy=that.y)]TJ/F31 7.9701 Tf 12.1126 0 Td[(this.y;returnMath.atan2dy,dx;g//EuclideandistancebetweenthispointandthatpointpublicDoubledistToPointthatfDoubledx=this.x)]TJ/F16 7.9701 Tf 12.2019 0 Td[(that.x;Doubledy=this.y)]TJ/F16 7.9701 Tf 12.2019 0 Td[(that.y;returnMath.sqrtdxdx+dydy;gpublicStringtoStringf//returnastringrepresentationofthispointreturnx+,+y;gg 282

PAGE 295

AppendixCRobotStatisticsCode robotstatsBaseDataCrunch.javapackagerobotstats;importjava.io.BufferedReader;importjava.io.FileNotFoundException;importjava.io.FileReader;importjava.io.IOException;importjava.util.Scanner;publicclassBaseDataCrunchf/@paramargs/publicstaticvoidmainString[]argsfStringControlFileName=args[0];StringDataFileName=args[1];tryf//Openthefilethatisthefirst//commandlineparameterSystem.out.printlnAttempting to open +DataFileName;FileReaderfstream;fstream=newFileReaderDataFileName;//ConvertourinputstreamtoaDataInputStreamBufferedReaderin=newBufferedReaderfstream;System.out.printlnFile Open!+DataFileName;System.out.printlnAttempting to open +ControlFileName;BufferedReadercin=newBufferedReadernewFileReaderControlFileName;System.out.printlnFile Open! +ControlFileName;//Continuetoreadlineswhile//therearestillsomelefttoread//Blowpasttheheader 283

PAGE 296

AppendixCContinued in.readLine;cin.readLine;Stringline=;Stringcline=;Integercount,ecount;count=ecount=0;DoublesumX,sumY,esumX,esumY;sumX=sumY=esumX=esumY=0.0;DoublesumSquareX,sumSquareY,esumSquareX,esumSquareY;sumSquareX=sumSquareY=esumSquareX=esumSquareY=0.0;whileline=in.readLine!=null&&cline=cin.readLine!=nullf//pulldownaline,thencrushit//System.out.printlnReadingaLine!;Scanners=newScannerline;Scannerc=newScannercline;//SkipTSs.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;s.next;//ValuesDoubleY=s.nextDouble;DoubleX=s.nextDouble;//ControlValuesc.next; 284

PAGE 297

AppendixCContinued c.next;DoubleCX=c.nextDouble;DoubleCY=c.nextDouble;ecount++;DoubleErrX=Math.absCX)]TJ/F16 7.9701 Tf 10.281 0 Td[(X;DoubleErrY=Math.absCY)]TJ/F16 7.9701 Tf 10.281 0 Td[(Y;esumX+=ErrX;esumY+=ErrY;esumSquareX+=ErrXErrX;esumSquareY+=ErrYErrY;ifecount%1000==0System.out.print.;gin.close;cin.close;DoubleemeanX=esumX/ecount;DoubleemeanY=esumY/ecount;DoubleestdX=Math.sqrtesumSquareX/ecount)]TJ/F16 7.9701 Tf 11.0724 0 Td[(emeanXemeanX;DoubleestdY=Math.sqrtesumSquareY/ecount)]TJ/F16 7.9701 Tf 11.0724 0 Td[(emeanYemeanY;System.out.printlnResults!;System.out.printlnError Mean: +emeanX+,+emeanY;System.out.printlnError STD: +estdX+,+estdY;gcatchFileNotFoundExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace; 285

PAGE 298

AppendixCContinued gcatchIOExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;ggg 286

PAGE 299

AppendixCContinued Cruncher.javapackagerobotstats;importjava.io.BufferedReader;importjava.io.FileNotFoundException;importjava.io.FileReader;importjava.io.IOException;importjava.util.Scanner;importjava.util.Vector;publicclassCruncherf/@paramargs/publicstaticvoidmainString[]argsfStringControlFileName=args[0];StringDataFileName=args[1];tryf//Openthefilethatisthefirst//commandlineparameterSystem.out.printlnAttempting to open +DataFileName;FileReaderfstream;fstream=newFileReaderDataFileName;//ConvertourinputstreamtoaDataInputStreamBufferedReaderin=newBufferedReaderfstream;System.out.printlnFile Open!+DataFileName;System.out.printlnAttempting to open +ControlFileName;BufferedReadercin=newBufferedReadernewFileReaderControlFileName;System.out.printlnFile Open! +ControlFileName;//Continuetoreadlineswhile//therearestillsomelefttoread//Blowpasttheheaderin.readLine; 287

PAGE 300

AppendixCContinued cin.readLine;Stringline=;Stringcline=;Integercount,ecount;count=ecount=0;DoublesumX,sumY,esumX,esumY;sumX=sumY=esumX=esumY=0.0;DoublesumSquareX,sumSquareY,esumSquareX,esumSquareY;sumSquareX=sumSquareY=esumSquareX=esumSquareY=0.0;whileline=in.readLine!=null&&cline=cin.readLine!=nullf//pulldownaline,thencrushit//System.out.printlnReadingaLine!;Scanners=newScannerline;Scannerc=newScannercline;//SkipTSs.next;//ValuesDoubleX=s.nextDouble;DoubleY=s.nextDouble;//ResidsDoublePX=s.nextDouble;DoublePY=s.nextDouble;//ControlValuesc.next;c.next;DoubleCX=c.nextDouble;DoubleCY=c.nextDouble;count++;ecount++;DoubleErrX=Math.absCX)]TJ/F16 7.9701 Tf 10.281 0 Td[(X;DoubleErrY=Math.absCY)]TJ/F16 7.9701 Tf 10.281 0 Td[(Y;sumX+=PX; 288

PAGE 301

AppendixCContinued sumY+=PY;esumX+=ErrX;esumY+=ErrY;sumSquareX+=PXPX;sumSquareY+=PYPY;esumSquareX+=ErrXErrX;esumSquareY+=ErrYErrY;ifcount%1000==0System.out.print.;gin.close;cin.close;DoublemeanX=sumX/count;DoublemeanY=sumY/count;DoubleemeanX=esumX/ecount;DoubleemeanY=esumY/ecount;DoublestdX=Math.sqrtsumSquareX/count)]TJ/F16 7.9701 Tf 10.916 0 Td[(meanXmeanX;DoublestdY=Math.sqrtsumSquareY/count)]TJ/F16 7.9701 Tf 10.916 0 Td[(meanYmeanY;DoubleestdX=Math.sqrtesumSquareX/count)]TJ/F16 7.9701 Tf 11.0724 0 Td[(emeanXemeanX;DoubleestdY=Math.sqrtesumSquareY/count)]TJ/F16 7.9701 Tf 11.0724 0 Td[(emeanYemeanY;System.out.printlnResults!;System.out.printlnResid Mean: +meanX+,+meanY;System.out.printlnResid STD: +stdX+,+stdY;System.out.printlnError Mean: +emeanX+,+emeanY;System.out.printlnError STD: +estdX+,+estdY;gcatchFileNotFoundExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace; 289

PAGE 302

AppendixCContinued gcatchIOExceptionef//TODOAuto)]TJ/F29 7.9701 Tf 6.801 0 Td[(generatedcatchblocke.printStackTrace;ggg 290