<%BANNER%>

A Study of Single and Multi-Robot Localization

Permanent Link: http://ufdc.ufl.edu/UFE0045287/00001

Material Information

Title: A Study of Single and Multi-Robot Localization a Manifolds Approach
Physical Description: 1 online resource (147 p.)
Language: english
Creator: Knuth, Joseph L
Publisher: University of Florida
Place of Publication: Gainesville, Fla.
Publication Date: 2013

Subjects

Subjects / Keywords: collaboration -- estimation -- geometry -- localizatoin -- riemannian
Mechanical and Aerospace Engineering -- Dissertations, Academic -- UF
Genre: Mechanical Engineering thesis, Ph.D.
bibliography   ( marcgt )
theses   ( marcgt )
government publication (state, provincial, terriorial, dependent)   ( marcgt )
born-digital   ( sobekcm )
Electronic Thesis or Dissertation

Notes

Abstract: We consider the problem of localizing autonomous robots when GPS is not available. Our work consists of two parts. First we examine how the estimation error grows with time when a mobile robot estimates its location from inter-time relative pose measurements without global position or orientation sensors. We show that in both 2-D or 3-D space, both the bias and variance of the position estimation error grows at most linearly with time (or distance traversed) asymptotically. The bias is crucially dependent on the trajectory of the robot. Exact formulas for the bias and the variance of the position estimation error are provided for two specific 2-D trajectories- straight line and periodic. Experiments with a P3-DX wheeled robot and Monte-Carlo simulations are provided to verify the theoretical predictions. A method to reduce the bias is proposed based on the lessons learned. We next consider a group of cooperating robots attempting to localize without the use of GPS. We propose a algorithm for estimating the 3-D pose (position and orientation) of each robot with respect to a common frame of reference. This algorithm does not rely on the use of any maps, or the ability to recognize landmarks in the environment. Instead we assume that noisy relative measurements between pairs of robots are intermittently available, which can be any one, or combination, of the following: relative pose, relative orientation, relative position, relative bearing, and relative distance. The additional information about each robots pose provided by these measurements are used to improve over self-localization estimates. The proposed method is based on solving an optimization problem in an underlying Riemannian manifold (SO(3) x R^3)^n(k) by a gradient descent law. The proposed algorithm is easily applicable to 3-D pose estimation, can fuse heterogeneous measurement types, and can handle arbitrary time variation in the neighbor relationships among robots. This algorithm is further refined by choosing a distribution for the various measurement types and developing a maximum likelihood estimator for collaborative localization. Simulations show that the errors in the pose estimates obtained using this algorithm are significantly lower than what is achieved when robots estimate their pose without cooperation. Results from experiments with a pair of ground robots with vision-based sensors reinforce these findings. Additionally, the question of trade-offs between cost (of obtaining a certain type of relative measurement) vs. benefit (improvement in localization accuracy) for the various types of relative measurements is considered. Finally, a set of simulations is present in which our proposed algorithm is compared against two state of the art collaborative localization algorithms. This comparison shows that the proposed method performs better when the error in orientation measurements is large, or when the time interval between inter-robot measurements is large. Finally, we propose an outlier rejection algorithm that functions as a preprocessing step for a pose graph collaborative localization algorithm, such as the one proposed earlier in this work, when all measurements are of the relative pose. Outliers are identified using only the information contained in the remaining relative measurements. In particular, no a priori distribution on the relative measurements is assumed, nor is any information about the absolute pose of the robots utilized. This outlier rejection algorithm exploits properties of pose measurements concatenated over simple cycles in the measurement graph to define an edge consistency cost such that large values are indicative of the presence of an outlier. A hypothesis test approach is then utilized to identify the set of likely outlying measurements. Simulations utilizing the proposed outlier rejection algorithm are presented. The outlier rejection algorithm is shown to successfully identify up to 95% of the outliers in the scenario considered, and successfully mitigate the effect of outliers on collaborative localization.
General Note: In the series University of Florida Digital Collections.
General Note: Includes vita.
Bibliography: Includes bibliographical references.
Source of Description: Description based on online resource; title from PDF title page.
Source of Description: This bibliographic record is available under the Creative Commons CC0 public domain dedication. The University of Florida Libraries, as creator of this bibliographic record, has waived all rights to it worldwide under copyright law, including all related and neighboring rights, to the extent allowed by law.
Statement of Responsibility: by Joseph L Knuth.
Thesis: Thesis (Ph.D.)--University of Florida, 2013.
Local: Adviser: Barooah, Prabir.

Record Information

Source Institution: UFRGP
Rights Management: Applicable rights reserved.
Classification: lcc - LD1780 2013
System ID: UFE0045287:00001

Permanent Link: http://ufdc.ufl.edu/UFE0045287/00001

Material Information

Title: A Study of Single and Multi-Robot Localization a Manifolds Approach
Physical Description: 1 online resource (147 p.)
Language: english
Creator: Knuth, Joseph L
Publisher: University of Florida
Place of Publication: Gainesville, Fla.
Publication Date: 2013

Subjects

Subjects / Keywords: collaboration -- estimation -- geometry -- localizatoin -- riemannian
Mechanical and Aerospace Engineering -- Dissertations, Academic -- UF
Genre: Mechanical Engineering thesis, Ph.D.
bibliography   ( marcgt )
theses   ( marcgt )
government publication (state, provincial, terriorial, dependent)   ( marcgt )
born-digital   ( sobekcm )
Electronic Thesis or Dissertation

Notes

Abstract: We consider the problem of localizing autonomous robots when GPS is not available. Our work consists of two parts. First we examine how the estimation error grows with time when a mobile robot estimates its location from inter-time relative pose measurements without global position or orientation sensors. We show that in both 2-D or 3-D space, both the bias and variance of the position estimation error grows at most linearly with time (or distance traversed) asymptotically. The bias is crucially dependent on the trajectory of the robot. Exact formulas for the bias and the variance of the position estimation error are provided for two specific 2-D trajectories- straight line and periodic. Experiments with a P3-DX wheeled robot and Monte-Carlo simulations are provided to verify the theoretical predictions. A method to reduce the bias is proposed based on the lessons learned. We next consider a group of cooperating robots attempting to localize without the use of GPS. We propose a algorithm for estimating the 3-D pose (position and orientation) of each robot with respect to a common frame of reference. This algorithm does not rely on the use of any maps, or the ability to recognize landmarks in the environment. Instead we assume that noisy relative measurements between pairs of robots are intermittently available, which can be any one, or combination, of the following: relative pose, relative orientation, relative position, relative bearing, and relative distance. The additional information about each robots pose provided by these measurements are used to improve over self-localization estimates. The proposed method is based on solving an optimization problem in an underlying Riemannian manifold (SO(3) x R^3)^n(k) by a gradient descent law. The proposed algorithm is easily applicable to 3-D pose estimation, can fuse heterogeneous measurement types, and can handle arbitrary time variation in the neighbor relationships among robots. This algorithm is further refined by choosing a distribution for the various measurement types and developing a maximum likelihood estimator for collaborative localization. Simulations show that the errors in the pose estimates obtained using this algorithm are significantly lower than what is achieved when robots estimate their pose without cooperation. Results from experiments with a pair of ground robots with vision-based sensors reinforce these findings. Additionally, the question of trade-offs between cost (of obtaining a certain type of relative measurement) vs. benefit (improvement in localization accuracy) for the various types of relative measurements is considered. Finally, a set of simulations is present in which our proposed algorithm is compared against two state of the art collaborative localization algorithms. This comparison shows that the proposed method performs better when the error in orientation measurements is large, or when the time interval between inter-robot measurements is large. Finally, we propose an outlier rejection algorithm that functions as a preprocessing step for a pose graph collaborative localization algorithm, such as the one proposed earlier in this work, when all measurements are of the relative pose. Outliers are identified using only the information contained in the remaining relative measurements. In particular, no a priori distribution on the relative measurements is assumed, nor is any information about the absolute pose of the robots utilized. This outlier rejection algorithm exploits properties of pose measurements concatenated over simple cycles in the measurement graph to define an edge consistency cost such that large values are indicative of the presence of an outlier. A hypothesis test approach is then utilized to identify the set of likely outlying measurements. Simulations utilizing the proposed outlier rejection algorithm are presented. The outlier rejection algorithm is shown to successfully identify up to 95% of the outliers in the scenario considered, and successfully mitigate the effect of outliers on collaborative localization.
General Note: In the series University of Florida Digital Collections.
General Note: Includes vita.
Bibliography: Includes bibliographical references.
Source of Description: Description based on online resource; title from PDF title page.
Source of Description: This bibliographic record is available under the Creative Commons CC0 public domain dedication. The University of Florida Libraries, as creator of this bibliographic record, has waived all rights to it worldwide under copyright law, including all related and neighboring rights, to the extent allowed by law.
Statement of Responsibility: by Joseph L Knuth.
Thesis: Thesis (Ph.D.)--University of Florida, 2013.
Local: Adviser: Barooah, Prabir.

Record Information

Source Institution: UFRGP
Rights Management: Applicable rights reserved.
Classification: lcc - LD1780 2013
System ID: UFE0045287:00001


This item has the following downloads:


Full Text

PAGE 1

ASTUDYOFSINGLEANDMULTI-ROBOTLOCALIZATION:AMANIFOLDS APPROACH By JOSEPHKNUTH ADISSERTATIONPRESENTEDTOTHEGRADUATESCHOOL OFTHEUNIVERSITYOFFLORIDAINPARTIALFULFILLMENT OFTHEREQUIREMENTSFORTHEDEGREEOF DOCTOROFPHILOSOPHY UNIVERSITYOFFLORIDA 2013

PAGE 2

c r 2013JosephKnuth 2

PAGE 3

ACKNOWLEDGMENTS IwouldliketoexpressmysinceregratitudetomyadviserDr. PrabirBarooah. Inadditiontoguidingmethroughoutmygraduatestudies,he establishedtheinitial directionofmyresearchandthenallowedmethefreedomtoco nsidertheprobleminmy ownway.Iwouldliketothankmycommitteemembers,Dr.Warre nDixon,Dr.CarlD. CraneIII,andDr.PaulRobinsonfortheirguidanceindirect ingthecourseofmywork.I amespeciallygratefultoDr.Robinsonforthetimehewaswil lingtodedicateintroducing metotheeldofdifferentialgeometry,abranchofmathemat icsthatprovedcriticalinmy work.Iwouldalsoliketothankmyfellowlabmateswhohave,t hroughtheirexample, constantlydrivenmetoworkharderandstrivetoaccomplish moreinmyownwork. Finally,Iwouldliketothankmywifeforhersupportandenco uragementthroughoutmy timehereattheUniversityofFlorida. 3

PAGE 4

TABLEOFCONTENTS page ACKNOWLEDGMENTS ..................................3 LISTOFTABLES ......................................7 LISTOFFIGURES .....................................8 ABSTRACT .........................................10 CHAPTER 1INTRODUCTION ...................................13 1.1Motivation ....................................13 1.2RelatedWork:SingleRobotErrorGrowth ..................18 1.3RelatedWork:CollaborativeLocalization ..................19 1.4RelatedWork:Outlierrejection ........................22 2ERRORGROWTHINPOSEESTIMATIONFROMDEADRECKONING ....23 2.1ProblemStatement ...............................23 2.2MainResults ..................................29 2.2.1GeneralTrajectories ..........................29 2.2.2DiscussiononTheorem2.1anditsProof ...............31 2.2.3Special2-DTrajectories ........................34 2.3ReducingtheBias ...............................37 2.4SimulationVerication .............................38 2.4.13-DSimulation .............................39 2.4.2Straight-Line2-DTrajectory ......................39 2.4.3PeriodicTrajectory ...........................44 2.5ExperimentalVerication ...........................46 2.5.1TestSet-Up ...............................46 2.5.2TestResults ...............................47 2.6ReducingtheBias ...............................49 2.7Summary ....................................51 3DISTRIBUTEDCOLLABORATIVE3DPOSEESTIMATION ...........53 3.1ProblemStatement ...............................53 3.1.1TheCollaborativeLocalizationProblem ...............53 3.1.2TheDistributedCollaborativeLocalizationProblem .........58 3.2CentralizedCollaborativeLocalizationAlgorithm ..............59 3.2.1CaseA:HomogeneousMeasurements(RelativePose) .......59 3.2.2CaseB:HeterogeneousMeasurements ...............63 3.3DistributedAlgorithm ..............................65 3.4SimulationResults ...............................67 4

PAGE 5

3.4.1AllMeasurementsareofRelativePose ................68 3.4.2HeterogeneousMeasurements ....................71 3.5ExperimentalResults .............................72 3.6Summary ....................................77 4EXISTINGMETHODSOFCOLLABORATIVELOCALIZATION .........79 4.1UnitQuaternions ................................79 4.2TheEuclideanPoseGraphOptimizationAlgorithm .............80 4.3TheImplicitExtendedKalmanFilterAlgorithm ...............84 4.3.1TheImplicit(Error-State)ExtendedKalmanFilter ..........84 4.3.2CollaborativeLocalizationUsingtheIndirectEKF ..........86 4.4SimulationResults ...............................90 4.5Summary ....................................93 5MAXIMUMLIKELIHOODESTIMATES .......................95 5.1TheMLEstimates ...............................95 5.2Simulationstudies ...............................99 5.2.1Performanceinasingleexperiment ..................100 5.2.2Effectofmeasurementnoise,andcomparisonwithstate -of-the-art cameranetworklocalizationalgorithm ................102 5.3Summary ....................................104 6OUTLIERREJECTIONONPOSEGRAPHS ...................105 6.1Problemstatement ...............................105 6.2OutlierRejectionAlgorithm ..........................105 6.3SlidingWindowApproximation ........................109 6.4Simulation ....................................111 6.4.1Justicationofthelog-normaldistribution ..............111 6.4.2OutlierRejection ............................112 6.5SummaryandFutureWork ..........................114 7CONCLUSIONANDFUTUREWORK .......................116 7.1Conclusion ...................................116 7.2FutureWork ...................................119 APPENDIX ASINGLEROBT:PROOFS ..............................121 BPRODUCTRIEMANNIANMANIFOLDS ......................130 B.1RiemannianManifolds .............................130 B.2ProductManifold ................................134 B.3ProofofTheorem3.1 .............................136 B.4GradientoftheCostFunction(5–9)forHeterogeneousMe asurements ..138 5

PAGE 6

REFERENCES .......................................141 BIOGRAPHICALSKETCH ................................147 6

PAGE 7

LISTOFTABLES Table page 5-1Theparametersofthemeasurementpdfsusedinallsimula tions ........100 6-1Theaveragenumberofmeasurements,outliers,measurem entsfalselyrejected, andoutliermeasurementsfalselyacceptedwhentheoutlier rejectionalgorithm issimulatedasapreprocessingstepofcollaborativelocal ization. ........114 7

PAGE 8

LISTOFFIGURES Figure page 2-1Aguretoexplainthenotation ...........................24 2-2The3-Dpathusedforthesimulationinthischapter ...............40 2-3Comparisonoftheoreticalboundsvs.simulationresult s .............40 2-4Comparisonoftheoreticalresultsforstraightlinevss imulationdata ......42 2-5Comparisonoftheoreticalresultsforstraightlinevss imulationcamera .....43 2-6Comparisonoftheoreticalresultsforperiodicmotionv ssimulation .......45 2-7Therobotsusedintheexperiments ........................45 2-8Schematicofthetestset-up .............................47 2-9Experimentalresultsforthe2-Dcircularmotioncase ...............48 2-10Arandomlygeneratedpathin2-Dusedtotestthebias-re ductionmethod. ...49 2-11Performanceofthebias-reductionmethod .....................50 3-1Thegraphcorrespondingtoagroupof 3 robotsattime k =3 ..........55 3-2Thelocalmeasurementgraphsfortherobots ...................66 3-3Thetrajectoriesandneighborrelationsusedinallsimu lations. .........69 3-4ComparisonbetweenthecentralizedanddistributedCLa lgorithm. .......70 3-5Simulationresultsstudyinglocalizationaccuracyv.s. numberofrobots .....71 3-6Simulationresultsstudyingthelocalizationaccuracyv .s.measurementtype ..73 3-7TwoPioneerP3-DXrobotsequippedwithcamerasandtargets .........74 3-8Experimental:Aplotofthelocationofrobot 1 usingRPGO ...........75 3-9Experimentalresultsstudyingthelocalizationaccurac yv.s.measurementtype. 77 4-1SimulationresultscomparingtheRPGOandEPGOalgorithms .........91 4-2SimulationresultscomparingtheRPGOandIEKFalgorithms ..........93 5-1Themagnitudeofthedifferencebetween p R and p R ...............98 5-2Asinglerealizationforagroupofrobotsusingvariousm easurementtypes ..101 5-3ComparisonofML-RPGOandthealgorithmproposedbyTrona ndVidalfor varyinglevelsofnoise ................................103 8

PAGE 9

6-1Acomparisonbetweentheestimatedpdfforthevaluesof ^ D ( e ) ascompared tothecorrespondinglog-likelihooddistributionpdf ................112 6-2Simulationresultsstudyingtheeffectofoutliersandth eoutlierrejectionalgorithm onlocalizationaccuracy ...............................113 9

PAGE 10

AbstractofDissertationPresentedtotheGraduateSchool oftheUniversityofFloridainPartialFulllmentofthe RequirementsfortheDegreeofDoctorofPhilosophy ASTUDYOFSINGLEANDMULTI-ROBOTLOCALIZATION:AMANIFOLDS APPROACH By JosephKnuth May2013 Chair:PrabirBarooahMajor:MechanicalEngineering Weconsidertheproblemoflocalizingautonomousrobotswhe nGPSisnot available.Ourworkconsistsoftwoparts.Firstweexamineh owtheestimationerror growswithtimewhenamobilerobotestimatesitslocationfr ominter-timerelativepose measurementswithoutglobalpositionororientationsenso rs.Weshowthatinboth2-D or3-Dspace,boththebiasandvarianceofthepositionestim ationerrorgrowsatmost linearlywithtime(ordistancetraversed)asymptotically .Thebiasiscruciallydependent onthetrajectoryoftherobot.Exactformulasforthebiasand thevarianceoftheposition estimationerrorareprovidedfortwospecic2-Dtrajector ies-straightlineandperiodic. ExperimentswithaP3-DXwheeledrobotandMonte-Carlosimula tionsareprovidedto verifythetheoreticalpredictions.Amethodtoreducetheb iasisproposedbasedonthe lessonslearned. Wenextconsideragroupofcooperatingrobotsattemptingto localizewithout theuseofGPS.Weproposeaalgorithmforestimatingthe3-Dpo se(positionand orientation)ofeachrobotwithrespecttoacommonframeofr eference.Thisalgorithm doesnotrelyontheuseofanymaps,ortheabilitytorecogniz elandmarksinthe environment.Insteadweassumethatnoisyrelativemeasure mentsbetweenpairs ofrobotsareintermittentlyavailable,whichcanbeanyone ,orcombination,ofthe following:relativepose,relativeorientation,relative position,relativebearing,and relativedistance.Theadditionalinformationabouteachr obotsposeprovidedbythese 10

PAGE 11

measurementsareusedtoimproveoverself-localizationes timates.Theproposed methodisbasedonsolvinganoptimizationprobleminanunde rlyingRiemannian manifold ( SO (3) R 3 ) n ( k ) byagradientdescentlaw.Theproposedalgorithmiseasily applicableto3-Dposeestimation,canfuseheterogeneousm easurementtypes,and canhandlearbitrarytimevariationintheneighborrelatio nshipsamongrobots.This algorithmisfurtherrenedbychoosingadistributionfort hevariousmeasurement typesanddevelopingamaximumlikelihoodestimatorforcol laborativelocalization. Simulationsshowthattheerrorsintheposeestimatesobtain edusingthisalgorithm aresignicantlylowerthanwhatisachievedwhenrobotsest imatetheirposewithout cooperation.Resultsfromexperimentswithapairofground robotswithvision-based sensorsreinforcethesendings.Additionally,thequestio noftrade-offsbetweencost (ofobtainingacertaintypeofrelativemeasurement)vs.be net(improvementin localizationaccuracy)forthevarioustypesofrelativeme asurementsisconsidered. Finally,asetofsimulationsispresentinwhichourpropose dalgorithmiscompared againsttwostateoftheartcollaborativelocalizationalg orithms.Thiscomparisonshows thattheproposedmethodperformsbetterwhentheerrorinor ientationmeasurementsis large,orwhenthetimeintervalbetweeninter-robotmeasur ementsislarge. Finally,weproposeanoutlierrejectionalgorithmthatfun ctionsasapreprocessing stepforaposegraphcollaborativelocalizationalgorithm ,suchastheoneproposed earlierinthiswork,whenallmeasurementsareoftherelati vepose.Outliersare identiedusingonlytheinformationcontainedintheremai ningrelativemeasurements. Inparticular,noaprioridistributionontherelativemeas urementsisassumed,noris anyinformationabouttheabsoluteposeoftherobotsutiliz ed.Thisoutlierrejection algorithmexploitspropertiesofposemeasurementsconcat enatedoversimple cyclesinthemeasurementgraphtodeneanedgeconsistency costsuchthatlarge valuesareindicativeofthepresenceofanoutlier.Ahypoth esistestapproachisthen utilizedtoidentifythesetoflikelyoutlyingmeasurement s.Simulationsutilizingthe 11

PAGE 12

proposedoutlierrejectionalgorithmarepresented.Theou tlierrejectionalgorithmis showntosuccessfullyidentifyupto 95% oftheoutliersinthescenarioconsidered,and successfullymitigatetheeffectofoutliersoncollaborat ivelocalization. 12

PAGE 13

CHAPTER1 INTRODUCTION 1.1Motivation LocalizationwithouttheuseofGPSisacrucialcapabilityfo ranyautonomous robot,astherearemanysituationsinwhichGPSsignalsareei therunavailable,oronly intermittentlyavailable.Theseincludeoperationinurba ncanyonsandtunnels,inside buildings,underwater,andextra-planetaryexploration. Insuchasituation,localization withrespecttoaninitialpositionistypicallyperformedu singacombinationofsensors thatareusedtomeasurerelativemotionbetweentwosuccess ivetimeinstants,and thenchainingthemtogetherinaprocessknownasdeadreckon ing.Inertialsensors (gyroscopesandaccelerometers),vision-basedsensors(c ameras,LIDARs,etc)and jointencoders(incaseofgroundvehicles)areexamplesofs ensorsthatcanbeused toobtainsuchmeasurements.Whenrobotsareoperatinginaco operativegroup, measurementsbetweenrobotsmayalsobeavailable,andwhen utilized,improvethe accuracyofthedeadreckoningestimates. Inthisworkwerstexaminethegrowthrateinthepositiones timationerror ofasinglerobotthatcannotdirectlymeasureeitheritsglo balpositionoritsglobal orientation.Specically,weanalyzethebiasandthevarian ceoftheerror.Therobot isequippedwithsensorsthatallowsittomeasuretherelati vepose(positionand orientation)betweenitscoordinateframesattwosuccessi vetimeinstants,butnot sensorsthatcanmeasureitsabsoluteposewithrespecttoag lobalcoordinateframe. Thatis,therobotmayhavesensorssuchaswheelodometers,I MUs,andcameras,but doesnothavesensorssuchasGPSandcompasses.Theabsolutep ositionhastobe estimatedfromthenoisyrelativeposemeasurementsthough deadreckoning. Whenrelativeposemeasurementsobtainedfromsensorsareco ncatenatedtoform anestimateoftherobot'spositioninaglobalframe,errors inindividualmeasurements accumulate.Overlongtimehorizons,theresultinglocatio nestimatesmaybecomequite 13

PAGE 14

poor.Thoughthisiswellrecognized,arigorousanalysisof theasymptoticgrowthrate seemstobelacking.Olsonet.al.[ 1 ]statesthatwithoutaglobalorientationsensor,the errorgrowssuper-linearlywithdistance,andpresentsexp erimentalevidencesupporting thatstatement.Theyfurtherstatethatpositionestimatio nerrorwillgrowas O ( s 3 = 2 ) where s isthedistancetravelled.Anumberofpapershaveclaimed,w ithoutproof,that thepositionestimationerrorgrowssuper-linearlywithdi stanceortimeintheabsence ofanabsoluteorientationsensor[ 2 – 7 ].Itisalsonotclearwhatismeantby“error”in thesereferences,whetheritisthemean,varianceorsomeot hermeasure.Aparametric statisticalmodelofthe2-normofthepositionestimatione rrorisproposedin[ 8 ],whose parametershavetobettedfrommeasurederror. Weshowinthisworkthattheasymptoticgrowthratesofbotht hebiasandvariance ofthepositionestimationerrorareupperboundedbylinear functionsoftime.Thus, evenwithoutanabsoluteorientationsensor,theerrorgrow th(bothbiasandvariance)is atmostlinear.Wealsoshowthatthevariancegrowthrateisl owerboundedbyalinear functionoftimeaswell,ifthevarianceofthetranslationm easurementissufciently large.Ouranalysisalsoprovidesinsightintothemechanis moferrorgrowth,particularly itsbias,thatdoesnotseemtohavebeenrecognizedsofar.In particular,weshowthat theexpectedvalueoftherobot'spositionestimateconverg estoapointirrespectiveof whethertherobotstaysinaboundedregionforalltimeornot .Anoutcomeofthisfactis thatthegrowthinthebiasdependscruciallyonthetypeofpa ththerobottraverseseven thoughtherobotdoesnothave-anddoesnotuse-information aboutitstrajectory. Thebiaswillbeboundedorunboundeddependingonlyonwheth ertherobotstays withinaboundedregionornot.Inaddition,theasymptotict rendsforthebiashold evenifthemeasurementsofrelativetranslationandrotati onareunbiased.Infact, theyholdeveniftherelativetranslationmeasurementsare error-free.Thebiasinthe translationmeasurementsthatarisefromvision-basedsen sorshasbeenatopicof 14

PAGE 15

research[ 9 10 ].However,thefactthatlargepositionestimationbiasmay occureven whenallmeasurementsareunbiasedhasnotbeenemphasizedi ntheliterature. Wenextconsiderthecasewhenagroupofrobots,eachwithout accesstoGPS, attempttolocalizethroughtheuseofdeadreckoning.Wepro poseamethodforfusing measurementsofvarioustypestoperformcollaborativeloc alizationthatimproves localizationaccuracyoverdeadreckoningalone.Asinthesi nglerobotcase,weassume allrobotsareequippedwithproprioceptivesensors(camer as,IMUs,etc.)allowing eachrobottomeasureitschangeinposebetweentimesteps.W erefertothesenoisy measurementsasinter-timerelativeposemeasurements.In theabsenceofanyother information,eachrobotcanperformlocalizationthroughd eadreckoningusingthese noisymeasurements.Wefurtherassumethateachrobotisequ ippedwithexteroceptive sensors,sothatnoisyrelativemeasurementsbetweenrobot smaybecomeavailable intermittently.Thesemeasurements,whichwecallinter-r obotrelativemeasurements, canbeone(oranycombination)ofthefollowing:relativepo se,relativeorientation, relativeposition,relativebearing,andrelativedistanc e,betweenapairsofrobots. Weprovideamethodtoperformcollaborativelocalizationb yfusingtheinter-timeand inter-robotrelativemeasurementstoobtainanestimateof theabsoluteposeofevery robot. Weprovidebothacentralizedanddistributedalgorithmfor collaborativelocalization. Inthecentralizedalgorithm,allthemeasurementsareassu medtobeinstantly availabletoacentralprocessor.Inthedistributedalgori thm,eachrobotperforms localizationusingmeasurementsfromon-boardsensorsand informationexchanged withneighboringrobots.Thecomplexityofthecomputation sperformedbyarobotis onlyafunctionofthenumberofitsneighborsatanygiventim e,notofthetotalnumber ofrobots.Thismakesthedistributedalgorithmscalableto arbitrarilylargegroupsof robots.Inaddition,thecommunicationrequirementsaresm all.Ateveryupdate,apairof neighboringrobotsneedstoexchangeonly(i)therelativem easurementbetweenthem 15

PAGE 16

and(ii)theircurrentposeestimates.Thoughthedistribut edalgorithmisnotequivalent tothecentralized,simulationsareprovidedthatindicate acomparableincreaseis localizationaccuracyisobtainedwitheach. Theabilitytofuseheterogeneousrelativemeasurementspr ovidesanimportant functionality.Inparticular,manypapers(see[ 11 – 15 ])requiremeasurementsofrelative pose,i.e.,positionandorientation,betweenrobotpairs. Whilerelativepositioncan beobtainedthroughstereovisionorlaserrangenders,obt ainingrelativeorientation isquitechallenging.Itoftenrequiresrobotstobeequippe dwithrecognizabletargets withknowngeometry;andeventhen,orientationmeasuremen tscanbequitenoisy. Infact,obtainingrelativepositionmeasurementswithste reovisionisalsochallenging duetotheeverdecreasingsizeofrobotslimitingthebaseli neavailableforstereo vision.However,evenifonlyonecameraofarobotseesaneig hboringrobot,abearing measurementcanbeobtained.Or,radiofrequency-basedtec hniquessuchasTOA (timeofarrival)measurementscanbeusedtomeasuredistan ceduringwireless communication.Withtheabilitytofusealltypesofrelative measurements,allavailable measurementscanbeutilizedforimprovementinlocalizati onaccuracy.Though[ 16 17 ] considerthecaseofheterogeneousmeasurements,theiralg orithmsarelimitedtothe 2-Dcase.Ourworkextendsto3-Dthecomparisonbetweenvari ousmeasurement typesinitiatedin[ 16 17 ]. Bothsimulationsandexperimentalresults(withapairofP3-D Xrobots)are presented.Ineachcase,theMonte-Carlomethodwasusedtoe mpiricallyobtain thebiasandstandarddeviationofestimationerrorbycondu ctingmultipletrials. Resultsshowthattheproposedcollaborativealgorithmsub stantiallyimproves upondead-reckoningevenwhentheteamconsistsofasmallnu mberofrobots.An examinationofimprovementvs.measurementtypeprovidesa basisfordeciding whetherthecostofthesensorsrequiredforobtainingcerta intypesofmeasurements arecommensuratewiththelocalizationaccuracytheyprovi de. 16

PAGE 17

Wenextcomparetheproposedalgorithm(referredtoastheRPG Oalgorithm) againsttwostateoftheartcollaborativelocalizationalg orithms.Therstbasedon thestandardposegraphoptimizationalgorithm,andthesec ondutilizinganindirect extendedKalmanlter.Animportantdistinctionbetweenthes estateoftheart algorithmsandthealgorithmpresentedhereisthenecessit yforparameterizationof theunderlyingspaceofrotations. Themethodproposedherealsobelongstotheposegraphbased approach:the estimationproblemisformulatedasanoptimizationdened byagraph,wherenodes representrobotposesatvarioustimesandedgesrepresenti nter-timeandinter-robot measurements.Thereisasubtle-butmajor-distinctionbet weenexistingposegraph optimizationmethods,whichwecallEuclideanposegraphopt imization(EPGO),and ourproposedmethod.Whileexistingmethodsuseavectorspac eparameterization oforientation(suchasthecomplexpartoftheunitquaterni onrepresentation)and thenapplyvector-spaceminimizationtechniquestosearch fortheminima,weperform theoptimizationdirectlyontheproductRiemannianmanifo ldinwhichtheproblem isnaturallyposedwithoutrelyingonaspecicparameteriz ation.Agradient-descent methodontheRiemannianmanifoldisthenusedforsearching fortheminima. Thegradientdescentalgorithmisindependentoftheparame terizationaswell;any parameterizationoftheorientationscanbeusedfornumeri calimplementationwithout affectingthesolutionobtained.Theadvantagesofdoingso arediscussedindetailin Section 4.2 (seeRemark 4.1 ).Asimilaruseofparameterizationisnecessaryinthe applicationofanindirectextendedKalmanlter(IEKF)tothec ollaborativelocalization problem. ForboththeEPGOandIEKFalgorithms,simulationsareprovideds tudythe samplebiasandvarianceinpositionestimationerrorthoug hMonte-Carloexperiments. Scenariosinwhichtheproposedalgorithmperformsbestarei dentied.Inparticular,in thecongurationsconsidered,theRPGOalgorithmoutperfor mstheEPGOalgorithm 17

PAGE 18

whenthemeasurementnoiseislarge,andoutperformstheIEKFa lgorithmwhenthe timebetweeninter-robotmeasurementsislarge. Thecollaborativelocalizationalgorithmisthenextended byidentifyingadistribution foreachmeasurementtypeandutilizingthemaximumlikelih oodestimatetolocalization thegroupofrobots.Thenon-vectorspacenatureofboththeo rientationandbearing measurementsisconsideredanddistributionschosenwithr especttotheappropriate measureonthegivenmanifolds.Simulationsarepresentedin dicatingamarked improvementinlocalizationaccuracywhenthemaximum-lik elihoodalgorithmisused. Thecollaborativelocalizationalgorithmspresentedinth eworkareanalogousto aleastsquaresoptimizationproblem.Sincelestsquaresopt imizationmethodsare oftensensitivetooutliers,identifyinganyoutliersinth emeasurementscanprovidea considerableincreaseinperformance.Ingeneral,thecoll aborativelocalizationproblem doesnotprovidepriordistributionsforthemeasurements, andthusoutliersmustbe identiedbasedonconsistencywithotherrelatedmeasurem ents.Inthisworkwe specicallyconsiderthecasewhenallmeasurementsareoft herelativeoration,orall areoftherelativepose.Insuchasituation,simplecyclesf oundinthemeasurement graphdescribingthelocalizationproblemcanbeusedtopro videameasureonthe consistencyofallmeasurements.Weassociatethoseedgeme asure'swithalog-normal distribution,andutilizeastandardGaussianoutlierreje ctionalgorithmtoidentifytheset oflikelyinconsistentmeasurements. 1.2RelatedWork:SingleRobotErrorGrowth ThepapersbySmithandCheesman[ 18 ],SuandLee[ 19 ],andWangand Chirikjian[ 20 ]derivedrecursiveexpressionsforthecovarianceofthepo seestimation errorbyassumingtheerrorsaresmall,sothatarstorderap proximationoftheBCH (Baker-Campbell-Hausdorff)formulaisvalid.Recently,Wa ngandChirikjian[ 21 ] developedarecursiveformulaforthecovarianceofthepose estimationerrorthat retainsthesecondordertermsintheBCHformula.Thepaper[ 22 ]examinesdead 18

PAGE 19

reckingerror'sprobabilitydensityfunctionfornon-holo nomicrobotsin2-D.Thework thatcomesclosesttooursinspiritis[ 8 ],whichproposedaparametricstatisticalmodel ofthe2-normofthepositionestimationerror.Someofthepar ametershavetobetted frommeasurederror.However,theworksmentionedabovedon otanalyzeasymptotic behavioroftheerror'smeanandvariance. Arelatedbodyofliteraturedealswithproblemofdevelopin gstateestimation techniquesforsystemswhosestates,aswellasthenoisymea surements,arein SO (3) or SE (3) (see[ 23 24 ]andreferencestherein).Theproblemofpositionestimati onofa mobilerobotwithnoisyrelativeposemeasurementsbetween successiveframes-one thatiscentraltothiswork-fallsintothiscategory.Howev er,ouraimisnottodevelopan estimationtechnique,buttoexaminethegrowthoferrorint hepositionestimatewhen successivenoisyrelativeposemeasurementsarechainedto gethertoobtainaglobal poseestimate. 1.3RelatedWork:CollaborativeLocalization Collaborativelocalizationhasbeenconsideredinthecont extofsimultaneous localizationandmapping(SLAM).Inoneclassofapproaches,r obotsexchangelocal mapswhicharealignedandmergedtoimproverobots'locatio nestimatesaswellas toimprovethemaps;see[ 25 26 ]andreferencestherein.Thisrequirestheabilityto identifycommonfeaturesindistinctmapsgeneratedbyther obots.Amethodbasedon posegraphsisdevelopedin[ 27 ]thatdoesnotrequireexchangeofmaps.In[ 28 ],robots exchangeimagesandanimplicitextendedKalmanlterisused toupdatethestateof eachrobotwhenacommonfeatureisfound. Recognizingcommonlandmarksindistinctmapsisoftenchal lenging.Inaddition, exchangingimagedataormapsbetweenrobotsrequireshighb andwidthcommunication. Asecondbodyofworkthereforeconsidersthecollaborative localizationproblemasone inwhichonlyrelativemeasurements(ofpose,position,ori entationetc.)betweenpairs ofrobotsareobtainedandusedtoimprovelocalizationaccu racyoverself-localization. 19

PAGE 20

Ourworkfallsintothiscategory,whichwetermcollaborati velocalizationfromrelative measurements. Collaborativelocalizationfromrelativemeasurementswa srstconsideredin[ 29 ], inwhichasetofrobotsweredividedintotwogroupssuchthat onlyonegroupwas allowedtomoveatatime.Thestationarygroupfunctionedas landmarksforthemoving group.Itwasshownthatforagroupofthreerobots,thismeth odoflocalizationled tolowererrorthanthatfromdeadreckoning.Subsequentwork allowedallrobotsto movesimultaneously,andcollaborativelocalizationwaso ftenaccomplishedthrough theuseofaKalmanlter(KF)oritsextensions.In[ 30 ]theauthormodelstheproblem asalinearestimationproblembyassumingeachrobot'sorie ntationisknownexactly. Measurementsofpositionbetweenrobotsarethenfusedwith thedeadreckoning positionestimatesforeachrobotthroughtheuseofaKalman lter.Theauthorfurther provesthatifthesensingandpositioningerrorsareindepe ndent,applicationoftheir algorithmwillalwaysimproveindividualrobotsestimates .In[ 12 ],theauthorsconsider thecollaborativelocalizationproblemwhennoisymeasure mentsoftheabsolute orientationsareavailable.AKalmanlterisusedtofusethe proprioceptiveand exteroceptivemeasurementsusinglinearizedupdateequat ions;thecomputations aredistributedamongallrobots.In[ 31 ]theauthorprovidesadistributedmethodof selectingmeasurementsbetweenrobotsthatminimizethelo calizationerrorforthe entireteam.Simulationsareprovidedthatshowselectingop timalmeasurements providessmallerlocalizationerroroverthecaseofrandom selection. In[ 32 ]theauthorsconsideragroupofmicroairvehicles(MAVs)inl evelightthat haveon-boardIMUsandarecapableofmeasuringtherelative positionofneighboring MAVs.AnEKFisusedtofusemeasurements,whileastudyofobserva bilityis performedprovidinginsightintowhatmeasurementsarenec essarytolocalizetheMAVs in2-D.In[ 15 ]theauthorsproposeamethodforestimatingthe2-Dposefor asetof collaboratingagentsbydecomposingtheproblemintomulti plephases:rsttheabsolute 20

PAGE 21

orientationsareestimatedusingrelativeorientationmea surements,thentheseareused alongwithrelativepositionmeasurementstoobtainabsolu tepositionestimates,and nallyanimprovedestimateoftheabsoluteorientationsar eobtainedfromtheposition estimates.Theestimationproblemineachphasearesolvedu singlinearestimation techniques.Theauthorsalsoproposeadistributedalgorit hmusingiterativeupdatesand provethattheestimatesfoundusingthedistributedalgori thmconvergeasymptotically tothosefoundusingthecentralizedalgorithm.In[ 33 ]theauthorsconsidertheproblem ofestimatingthe2-Dposeforasetofanonymousroadvehicle s.Eachvehicleusesan extendedKalmanlter(EKF)toestimateitsownpositionandvel ocityalongwiththe positionandvelocityforeveryvehicleithasseenrecently .Identicationofvehiclesis performedusingtheMahalanobisdistancetocomparemeasur edandestimatedpose, equatingtwovehiclesonlyifthedistancefallsbelowsomet hreshold.Thenalestimate oftheposeforagivenvehicleisobtainedbyfusingthestate vectorsforallneighboring vehicles. Acentralizedalgorithmforcollaborativelocalizationis presentedin[ 14 ]thatis applicabletoboth2-Dand3-D.Anonlinearcostfunctionisp roposedthat,when minimized,providesamaximumlikelihoodestimateofthepo seforeachrobot.The nonlinearoptimizationproblemissolvedusingstandardop timizationtechniques.In[ 34 ] analgorithmforcollaborativelocalizationin3-Dbylinea rizingtherelationbetweenthe posemeasurementsispresented.ResultsfromaMonte-Carlo simulationareprovided showingimprovedlocalizationaccuracywithincreasingnu mberofrobots. Leungetal.considerstheproblemofequivalencybetweence ntralizedand decentralizedcollaborativelocalizationalgorithms[ 35 ].Nospecicalgorithmis proposed.Insteadtheyallowforanarbitraryalgorithmto twithintheirframework. TheyshowthatgiventheMarkovpropertyholdsfortheestima tedstates,thetimeat whichpastmeasurementscanbediscardedcanbedeterminedi nadecentralized mannersuchthatthedistributedalgorithmwillbeequivale nttothecentralizedalgorithm. 21

PAGE 22

Theaforementionedpapersassumerelativeposemeasuremen tsbetweenrobots. Inmanyapplications,thesemaybedifcultorexpensivetoo btain.Tothatend,[ 16 ] extendstheparticlelterapproachfor2-Dcollaborativel ocalizationwhenrobotstake turnsmovingtoincludenotonlyrelativeposemeasurements ,butalsomeasurements oftherelativeorientation,position,bearing,anddistan ce.Monte-Carlosimulations areusedtocomparetheeffectdifferenttypesofinter-robo tmeasurements,alongwith thenumberofrobots,hasonlocalizationaccuracy.In[ 17 ]theauthorsexpandtheuse oftheEKFtoestimatethe2-Dposeusingmeasurementsofthebea ring,distance,or orientationbetweenrobots.Simulationsareprovidedthati ndicateanycombinationof thesemeasurements,withtheexceptionofdistancealone,p rovidessufcientadditional informationtoimproveoverdeadreckoning. 1.4RelatedWork:Outlierrejection Manycollaborativelocalizationalgorithmsattempttodet ectdataassociation errorsorfalselyidentiedloopclosureevents.Inparticu lar,[ 36 – 40 ]eachattemptto performsimultaneouslocalizationandmappingwhileremai ningrobusttooutliers. Howeverineachoftheworksmentionedabove,outlieridenti cationisnotgrantees,so apre-processingalgorithmtoidentifyoutliersisbeneci al.Inthisworkwepresentsuch apre-processingalgorithm. Theproblemofidentifyingoutlierspriortoutilizingloca lizingwasalsoconsidered by[ 41 ].Theauthorsutilizealeastsquaresapproachtoposegraph optimization,with theinclusionofanadditionalstateforeachmeasurementin dicatingweatherthat measurementshouldbeincluded.Theoptimalsolutiontothe modiedposegraph problemindicateswhatmeasurementsarelikelytobeoutlie rs. 22

PAGE 23

CHAPTER2 ERRORGROWTHINPOSEESTIMATIONFROMDEADRECKONING Inthischapterweexaminethegrowthrateinthepositionest imationerrorofa robotthatcannotdirectlymeasureeitheritsglobalpositi onoritsglobalorientation. Specically,weanalyzethebiasandthevarianceoftheerror .Therobotisequipped withsensorsthatallowsittomeasuretherelativepose(pos itionandorientation) betweenitscoordinateframesattwosuccessivetimeinstan ts,butnotsensorsthatcan measureitsabsoluteposewithrespecttoaglobalcoordinat eframe. Therestofthechapterisorganizedasfollows.Section 2.1 preciselyformulatesthe problemunderstudy,andSection 2.2 statesthemainresults.Amethodforreducing thebiasinpositionestimationerrorispresentedinSection 2.3 .Simulationverication ispresentedinSection 2.4 andexperimentalvericationispresentedinSection 2.5 Finally,simulationsstudyingthebias-reductionalgorit hmpresentedinSection 2.3 are presentedinSection 2.6 2.1ProblemStatement Wemeasuretimewithadiscreteindex k =0,1,... .Sensorsusedforrelative localizationofautonomousvehiclesyieldanestimateofth epositionandorientation ofthevehicleattime k relativetothatintheprevioustimeinstant, k 1 .Thatis,they produceanestimateoftherelativeposebetweenframesatta chedtotherobotattwo successivetimeinstants.Let R kk +1 betherotationbetweenthelocalframesattachedto therobot'sbodyattime k and k +1 .Thatis,if u k isavectorexpressedinthevehicle's frameattime k and u k 1 isthesamevectorexpressedinthevehicle'sframeattime k 1 ,then u k 1 = R k 1 k u k .Thisnotationisadoptedfrom[ 42 ].Wewillrefertothe framethatisattachedtothevehicleattime k asthe“frame k ”.Similarly,let t ki j be therelativetranslationfromtheframe i totheframe j ,expressedintheframe k .The rotation R k 1 k 2 SO ( d ) isusuallyexpressedasa d d matrixfor d 2f 2,3 g ,while t ki j isavectorin R d .Withoutlossofgenerality,thecoordinateframethatisatt achedtothe 23

PAGE 24

pathR 01 R 12R 01 t 10,1R 12 t 21,2t 00,2 (= R 01 t 10,1 + R 01 R 12 t 21,2 )Figure2-1.Aguretoexplainthenotation:arobot'spath(s hownindashedblueline)in 2-Dandassociatedrelativeposesbetweentimeinstants. t 0k 1, k isthe translationbetweentheframes k 1 and k ,expressedintheglobalframe 0 and t kk 1, k isthesamevectorexpressedinthelocalframe k .Thematrix R 0k istherotationbetweenframe 0 and k ,sothat R 0k t kk 1, k isthetranslationfrom k 1 to k expressedintheglobalframe 0 robot'sbodyattheinitialtime k =0 isusedastheglobalcoordinateframe.Wedenote therotationfromframe k totheglobalcoordinateframe(frame 0 )by R 0k .Similarly,the translationfromframe k 1 to k expressedintheglobalcoordinateframeisdenotedby t 0k 1, k .Thepositionoftherobotatanygiventime n isthevector t 00, n Withrelativeposesensorssuchasinertialsensors,cameras ,andwheelodometers, themeasurementsavailableattime k areestimatesoftherelativetranslationfrom k 1 toframe k expressedinframe k ,i.e.,of t kk 1, k ,andtherotationbetweentheframes k 1 and k .i.e.,of R k 1 k .Thetranslationfrom k 1 to k ,for k 1 ,expressedintheglobal coordinateframeis t 0k 1, k = R 0k t kk 1, k where R 0k = R 01 R 12 ... R k 1 k Anexampleofarobotspathalongwithitscorrespondingrelat iveposemeasurements canbeseeninFigure 2-1 .Estimatesaredenotedbyhatsontopofthecorresponding symbols,anderrorsbytildes,sothat ^ R k 1 k and ^ t kk 1, k arethenoisyestimatesof R k 1 k 24

PAGE 25

and t kk 1, k ,andthecorrespondingerrors ~ R k 1 k and ~ t kk 1, k aredenedas ~ R k 1 k :=( R k 1 k ) 1 ^ R k 1 k ~ t kk 1, k := ^ t kk 1, k t kk 1, k (2–1) Theabsolutepositionoftherobotattime k isdeterminedbyexpressingtherelative positionmeasurementsateachtimestepintheglobalcoordi nateframeandadding themtogether.Themeasurementofthetranslationfromfram e k 1 to k expressedin theglobalcoordinateframe,whichisdenotedby ^ t 0k 1, k ,is ^ t 0k 1, k := ^ R 0k ^ t kk 1, k (2–2) where ^ R 0k isanestimateof R 0k ,whichiscomputedfromtherelativerotationestimatesas ^ R 0k = k Y i =1 ^ R i 1 i (2–3) Finally,theestimateofthepositionattime n intheglobalcoordinateframe 0 isobtained byaddingtherelativetranslationestimatesaftertransfo rmingthemtoframe 0 : ^ t 00, n := n X k =1 ^ t 0k 1, k (2–4) Theerrorbetweentheestimatedpositionandthetruepositi onattime n is e ( n ):= t 00, n ^ t 00, n (2–5) Thegoalofthischapteristostudyhowthemeanandcovarianc eoftheposition estimationerror e ( n ) scaleswiththetimeindex n .Iftherobot'sspeedisupperandlower boundedbytwoconstants,thentheasymptotictrendswithti meareequivalenttothose withdistancetravelled.Thereforeweonlystudyscalingwi ththetimeindex n Thestraight-forwarddead-reckoningformula( 2–4 )maynotbeusedinpractice. Typicallyaltering-basedalgorithmisusedtofuserelati veposemeasurementswith thepredictionsofamodeloftherobot'smotion.Therearema nyvariationspossiblein 25

PAGE 26

termsofassumedmodel,statesandinputmeasurements;see[ 43 ]foracomparison amongsomeofthem.Thisrendersexaminingthemechanismofe rrorpropagationand establishingasymptoticgrowthratesofsuchalgorithmsin tractable.Thereforeweadopt thesimpledead-reckoningmodelthatstillcapturestheess entialfeaturesoflocalization fromrelativeposemeasurements.Wewishtoemphasizethatt heestimationerror resultingfromtheestimationmethoddescribedabovewillh avethesameasymptotic trendasthatofalteringtechniquethatusesakinematicmo deloftherobotmotion. Thereasonisthatakinematicmodelessentiallyproducesan independentnoisy measurementoftherelativepose.Thus,ourinvestigations areusefulinanalyzing asymptoticperformanceofawiderclassofestimationtechn iques.Onesituationwhere ourmodelisnotappropriateiswhenloop-closureisusedtoa ugmentlocalization[ 44 ]. Wefocusonsituationswhereloopclosureisnotapplicable, e.g.,anunmannedaerial vehicleyinginanexpansiveenvironmentsothatitmaynotc omebacktoitsearlier positions. Tostatetheassumptionsonmeasurementerrorstatistics,w eestablishafew conventions.Arotationmatrix R 2 SO (3) ,wherethespecialorthogonalgroup SO (3) is thesetof 3 3 realorthogonalmatriceswithunitdeterminant,canberepr esentedbythe exponentialmap: R = e s ,where s isthe 3 3 skew-symmetricmatrixcorresponding tothevector 2 R 3 [ 45 ,Chapter2].Amatrixin SO (2) isuniquelyspeciedbyan angle 2 [ ) .Arandomrotationmatrix R 2 SO (3) (resp. SO (2) )cantherefore bespeciedbyarandomvector 2 R 3 (resp.ascalarr.v. ).Wesaythattworandom rotationmatrices R 1 R 2 2 SO (3) areindependentiftheircorresponding 1 and 2 are independentrandomvectors.For SO (2) ,independenceofrotationsisdenedasthe independenceofthescalarrandomvariables 1 2 thatuniquelydeterminethem.If R 1 and R 2 areindependent,everyentryofthematrix R 1 isindependentofeveryentryof R 2 .Similarly,wesaythatarotation R 1 2 SO (3) (resp., SO (2) )andarandomvector 26

PAGE 27

t 2 R 3 (resp. R 2 )areindependentif 1 (resp., )and t areindependent.Inthiscase, too,everyentryof t isindependentofeveryentryof R Inthischapter,weuse E[ R ] (forarandomrotationmatrix R )todenotethematrix whose i j -thentryis E[( R ) i j ] ,i.e.,theexpectedvalueofthe i j -thentryof R .Asadirect resultofthisconvention,if R 1 2 SO ( d ) isindependentof R 2 2 SO ( d ) andof t 2 R 3 ,then E[ R 1 R 2 ]=E[ R 1 ]E[ R 2 ] and E[ R 1 t ]=E[ R 1 ]E[ t ] Inthesequel, Tr [ ] standsfortraceofamatrix,and kk q denotesthe(induced) q -normofa(matrix)vector.Whenthesubscriptisomitted,itd enotesthe2-norm. Westatethefollowingassumptionsforuseintherestofthec hapter. Assumption2.1. 1.Therobot'sspeedisuniformlybounded.Morespecically ,there existsaconstant > 0 suchthat k t kk 1, k k 2.Thetranslationmeasurementerrors ~ t kk 1, k formasequenceofindependent randomvectors,withmean b k :=E[ ~ t kk 1, k ] andcovariance P k := Cov ( ~ t kk 1, k ~ t kk 1, k ) thatareuniformlybounded.Thatis,thereexistscalarcons tants b p p suchthat 0 k b k k b and 0 p Tr [ P k ] p < 1 forall k 3.Therotationmeasurementerrors ~ R k 1 k formasequenceofindependentrandommatrices.Therotationandtranslationmeasurementerr ors ~ R j 1 j and ~ t kk 1, k aremutuallyindependentif j 6 = k ,andpossiblydependentwhen j = k ,with E[ ~ R k 1 k ~ t kk 1, k ]=: k 2 R d .Thereexistsascalar suchthat k k k forall k 4.Therelativetranslationmeasurementerrors f ~ t kk 1, k g 1k =1 areuniformlyabsolutely integrable,i.e.,thereexistsascalar sothat k < 1 forall k where k :=E k ~ t kk 1, k k 5.Therotationmeasurementerrors ~ R k 1 k areidenticallydistributed,sothateach ~ R k 1 k hasthesamedistributionasthatofsomematrix ~ R 2 SO ( d ) d 2f 2,3 g Moreover, ~ R isnotdegenerate,i.e.,itspdf(probabilitydistribution function)isnot concentratedonasetofmeasurezero. 27

PAGE 28

Apartfromtheassumptionsonindependenceofmeasurementer rors,theother assumptions,inparticularthoseontheexistenceofthepar ameters b p p ,are triviallysatisedinanypracticalscenario.Finitenesso fthedisplacement andbias norm b areeasytosee;theparameters p and p aresimplythelowerandupperbounds ontheeigenvaluesof P k .The d -dimensionalvector k isameasureofthecorrelation betweenthetranslationandrotationmeasurements,andthe parameter isanupper boundonthemagnitudeofthecorrelation.Weallowtherelat ivetranslationandrotation measurementerrorsataparticulartimeinstanttobestatis ticallydependent,since thismayhappenifthereisoverlapbetweenthesensorsuiteu sedtoobtainthesetwo measurements.Theparameter isakintoanupperboundonthesumofbiasand varianceofthetranslationmeasurementerror.Toseethis, considernot E[ k ~ t kk 1, k k ] but E[ k ~ t kk 1, k k 2 ] ,whichisthetraceofthesecondmomentoftranslationmeasu rement error ~ t kk 1, k .Sincethesecondmomentisthesumofcovarianceandrstmome nt,an upperboundon E[ k ~ t kk 1, k k 2 ] isalsoanupperboundonsumofmeanandvariance(more precisely,on k b k k 2 +Tr [ P k ] )ofthetranslationmeasurementerror. Thefollowingtechnicalresultiscrucialforthemainresul tsofthischapterandwill berequiredforthesubsequentdiscussions.Wethereforest ateithere;theproofis providedintheAppendix.Proposition2.1. Let R bearandomrotationmatrixwithdistributiondenedover SO ( d ) d 2 andlet E[ R ] bethe d d matrixwhose i j -thentryistheexpectedvalueofthe i -thentryof R .Wehave k E[ R ] k 1 ,andtheinequalityisstrictifthedistributionof R is notdegenerate 1 2 1 Recallthatwesaythedistributionof R isdegenerateifitspdfis 0 everywhere exceptpossiblyinasetofmeasure 0 28

PAGE 29

Duetoitsusefulnessinlaterdiscussions,wedene R :=E[ ~ R ]. (2–6) Recallthat ~ R isarotationmatrixthathasthesamedistributionasallthe rotationerrors ~ R kk +1 k =1,... .ItfollowsfromProposition 2.1 thatunderAssumption 2.1 1 >r := k R k (2–7) Accordingtotheconventionusedinthischapter,ingeneral E[ R ] = 2 SO ( d ) even if R 2 SO ( d ) .Itisimportantthatthenotation E[ R ] isnottobeunderstoodasthe expectationoftherandomvariable R withadistributiondenedover SO ( d ) ,which wedenoteby R ,sothat R 2 SO ( d ) .Wecall R the“Lie-groupmean”of R .We callanestimate ^ R ofatruerotation R unbiasedif ^ R = R .Aresultoftheadopted conventionisthatforanunbiasedestimator ^ R of R ,ingeneral E[ ^ R ] 6 = R .Thereasonthe quantity E[ R ] ismoreusefulforthisworkthan R isthatwhen R and t areindependent, E[ Rt ]=E[ R ]E[ t ] butingeneral E[ Rt ] 6 = R E[ t ] .Thebiasintranslationmeasurements obtainedfromvision-basedsensorshasbeenthesubjectofr esearch[ 9 10 ].Thebiasin rotationmeasurement,ontheotherhand,seemtohavedrawnl imitedattention.In[ 9 ], theerrorin3-Drotationisdescribedintermsofthecorresp ondingEulerangles,and biasinrotationisalsodenedintermsofthebiasintheEuler angles.Analternate denitionof3-Drotationerrorintermsofa3-vector(invol vingangleandaxisofrotation) isusedin[ 46 ],butthequestionofitsbiasisnotdiscussed. 2.2MainResults 2.2.1GeneralTrajectories Beforestatingtheresult,wereviewtheasymptotic O ,n, notation.Fortwo scalar-valuedfunctions f ( n ), g ( n ) takingnon-negativeintegerarguments,thenotation f ( n )= O ( g ( n )) meansthereexistsapositiveinteger n 1 andapositiveconstant c 1 so that f ( n ) c 1 g ( n ) forall n n 1 .Thenotation f ( n )=n( g ( n )) meansthereexistsa 29

PAGE 30

positiveinteger n 2 andapositiveconstant c 2 sothat f ( n ) c 2 g ( n ) forall n n 2 .The notation f ( n )=( g ( n )) meansboth f ( n )=n( g ( n )) and f ( n )= O ( g ( n )) hold. Theorem2.1. Considerarobotmovingina2-Dor3-DEuclideanspacethatper forms positionestimationfromrelativeposemeasurementsasdes cribedinSection 2.1 .Under Assumption 2.1 ,thefollowingstatementshold,where b p p areparameters denedinAssumption 2.1 and r isdenedin ( 2–7 ) 1.Thebiasinthepositionestimationerrorsatises k E[ e ( n )] k = O ( n ) .Inparticular, max 0, k t 00, n k 1 r n 1 r ( r + ) k E[ e ( n )] kk t 00, n k + 1 r n 1 r ( r + ) (2–8) 2.Thepositionerrorcovariancesatises Tr [ Cov ( e ( n ), e ( n )) ] = O ( n ) ,withupper boundgivenby Tr [ Cov ( e ( n ), e ( n )) ] 0 1+ r 1 r n (2–9) where 0 =max ( 2 +2 b + p + b 2 ),( + r )( + b ) (2–10) Iffurthermore p 2 b + 2 +2 ( + =r )( + b ) 1 r (2–11) then Tr [ Cov ( e ( n ), e ( n )) ] =( n ). 2 Beforediscussingtheimplicationsofthetheorem,wepresen taresultintheformof alemmathatisusefulinboththediscussionandtheproofoft hetheorem.Theproofof thelemmaisprovidedintheAppendix. 30

PAGE 31

Lemma1. UnderAssumptions 2.1 ,therstandsecondmomentofthepositionestimate satises k E[ ^ t 00, n ] k 1 r n 1 r ( r + ),E[ k ^ t 00, n k 2 ] 0 1+ r 1 r n where 0 isdenedin ( 2–10 ) .Moreover,ifcondition ( 2–11 ) issatised,thenwehave E[ k ^ t 00, n k 2 ]=( n ) 2 2.2.2DiscussiononTheorem 2.1 anditsProof Theorem 2.1 ,andinparticulartheupperboundin( 2–8 ),showsthatiftherobot's motionisconnedtoaboundedregion,thenthebiasinthepos itionestimationerror staysuniformlyboundedbyaconstant: k E[ e ( n )] k = O (1) .Iftherobotmoveswith aconstantspeedandwithaconstant(absolute)orientation ,thenitspositiongrows linearlywithtime.Inthiscasethetheoremindicatesthatt hebiasgrowslinearlywith time: k E[ e ( n )] k =( n ) ,sincenowboththeupperandlowerboundsareasymptoticall y linearintime.Thisimpliesthattheasymptotictrendofthe biasiscruciallydependenton therobot'sdisplacement. Thisdependencyofthebiasontherobot'strajectoryisacon sequenceofthefact thatthattheestimatedpositionisalwaysboundedinmean,e veniftherobotismoving outtoinnity,whichfollowsfromLemma 1 .Toobtainanintuitiveunderstandingof Lemma 1 ,werstnotethattheestimatedpositionissimplythesumof thetranslations aftertransformingthemtothecommonglobalcoordinatefra me 0 ;see( 2–4 ).Taking expectationonbothsidesof( 2–4 ),weobtain E[ ^ t 00, n ]=E[ ^ t 00,1 ]+E[ ^ t 01,2 ]+ +E[ ^ t 0n 1, n ]. (2–12) 31

PAGE 32

The k th terminthesumabove,assumingrotationandtranslationmea surementsare independent,is E[ ^ t 0k 1, k ]=E k 1 Y 0 ^ R ii +1 ^ t kk 1, k # = k 1 Y 0 E[ ^ R ii +1 ] E[ ^ t kk 1, k ] = k 1 Y 0 R ii +1 R E[ ^ t kk 1, k ]. Themagnitudeofthistermisoforder r k ,sinceitinvolves k productsof R ,eachof whichhasanormequalto r .Since r< 1 (see( 2–7 )),thesum( 2–12 )isbounded forall n .Theexpectedvalueofthepositionestimatethereforeconv ergestoapoint. Noticethatthebounds( 2–8 )onthebiasdoesnotdependontheerrorinthetranslation measurements.Theconclusionsdrawnaboveremainthesamee veniftherotationand translationmeasurementsareunbiased,i.e., ~ R = I b k =0 ,andinfact,evenifthe translationmeasurementsarecompletelyerrorfree, ~ t kk 1, k =0 Thediscussionabovecanbesummarisedintothefollowingco nclusionsaboutthe bias: (i)Forlargetimeindex n ,themaincontributionstothebiasinthepositionestimate arethedisplacementoftherobotandtheerrorsintherelati verotationmeasurements. (ii)Theasymptoticscalingofthebiasdoesnotchangeevenw henthetranslationand rotationmeasurementsareunbiased,andinfacteveniftran slationmeasurements arecompletelywithouterror. Therstconclusioniswellknown,andishardlysurprising. However,conclusion(ii) doesnotseemtoberecognizedintheliterature. Thevariancegrowthratedoesnotseemtobesensitivetothet rajectoryofthe robot.Furthermore,unlikethebias,thevariancecangroww ithoutboundwhenthe robot'strajectoryisconnedtoaboundedregion.We'llsee evidenceofthislaterin simulationsandexperimentsreportedinSections 2.4 and 2.5 .Webelievethatthe sufcientcondition( 2–11 )isconservative,andisanartifactofourprooftechnique. 32

PAGE 33

Thecondition( 2–11 )isusuallynotsatisedinpracticesinceitrequiresavery large translationmeasurementerror.Yetthepositionestimatio nerrorvarianceseemstobe ( n ) insimulationsandexperimentsreportedinSection 2.5.2 Theresultsofthetheoremareincontrasttotheprevalentbe liefintheliterature thattheerrorgrowthissuperlinearintimeifabsoluteorie ntationmeasurementsare notavailable[ 1 – 7 ].Thetheoremshowsthatevenwithoutabsoluteorientation sensors, localizationerror-ormorepreciselyitsbiasandvariance -growsatmostlinearlywith time.Webelievethatthebeliefaboutsuperlineargrowthca meaboutfromthefact thatexperiments/simulationswerenotconductedlongenou ghtodrawreasonable conclusionaboutasymptotictrends.Throughtherootcause isthegeometricdecaydue to r ,since r isusuallyquitecloseto 1 ,thereisaninitialperiodwheretheerrorgrows sharplyuntilthegeometricdecaykicksinandthelineartre ndbecomesobvious.More insightintothisphenomenonwillbeobtainedlaterinSectio n 2.2.3 thatdiscusses2D trajectories(seeinparticularTheorem 2.2 ). TheproofofTheorem 2.1 ,presentednext,followsfromLemma 1 inastraightforward manner.Proof2.1.1. 2.1 Itfollowsfrom ( 2–5 ) ,byapplyingthetriangleinequalitythat k E[ e ( n )] kk t 00, n k + k E[ ^ t 00, n ] k (2–13) k E[ e ( n )] k max 0, k t 00, n kk E[ ^ t 00, n ] k (2–14) FromLemma 1 ,wehavethat k E[ ^ t 00, n ] k isupperboundedandsotherststatement followsimmediatelyfrom ( 2–13 ) and ( 2–14 ) 33

PAGE 34

=2 c r T ( I c R ) 1 R r +Tr P + bb T +(2 b T + r T )( I c R ) 1 (2–15) ( n )= r T ( I c R ) 2 I 4 c R +2( c R ) 2 +2( c R ) n +1 r 2 b T ( I c R ) 2 ( I ( c R ) n ) + b T ( I c R ) 1 [ I ( c R ) n ] r r T ( I c R ) 2 [ I ( c R ) n ] k ( I c R ) 1 ( I ( c R ) n )( c R r + ) k Toprovethesecondstatement,notethat Tr [ Cov ( e ( n ), e ( n )) ] =Tr Cov ( ^ t 00, n ^ t 00, n ) =Tr E[ ^ t 00, n ( ^ t 00, n ) T ] E[ ^ t 00, n ]E[ ^ t 00, n ] T =E[( ^ t 00, n ) T ^ t 00, n ] k E[ ^ t 00, n ] k 2 E[( ^ t 00, n ) T ^ t 00, n ]. Since k E[ ^ t 00, n ] k = O (1) ,thesecondstatementfollowsfromLemma 1 2.2.3Special2-DTrajectories Inthissectionweprovidenon-asymptoticresultsontheerr orgrowthforthespecial casewhenthemotionoftherobotisconnedtoa2Dplaneandit strajectoryislimited totwoparticulartypes.Inthe2-Dscenario ^ t ij k t ij k 2 R 2 and R ij ^ R ij 2 SO (2) forevery i j k .The x and y axesofaCartesiancoordinateframethatliesonthisplanea ndis attachedtotherobot'sbodyattheinitialtime k =0 isusedastheglobalcoordinate frame.Inthe2-Dscenario,therobot'sorientationattime n canbeuniquelydescribed byanangle 0, n 2 [ ) ,whichdescribesrotationofitslocalframeaboutthe z -axis oftheglobalframe.Therelativerotationbetweentheframe s k 1 and k isuniquely determinedbytheanglebywhichtheframe k 1 hastoberotatedinthecounter clockwisedirectiontoreachframe k ,whichwedenoteby k 1, k .Figure 2-1 showsan example.Anoisymeasurementoftherelativerotation,deno tedby ^ k 1, k ,isassumed availableattime k .Theerrorintherelativerotationmeasurementis ~ k 1, k := ^ k 1, k k 1, k (2–16) 34

PAGE 35

Forfutureuse,wedene f R :[ ) SO (2) as f R ( ):= 0B@ cos sin sin cos 1CA Thematrix R k 1 k thatdescribestherelativerotationbetweentheframes k 1 and k is thereforegivenby R k 1 k = f R ( k 1, k ) .Itcanbeshownfromthedenition( 2–1 )that ~ R k 1 k = f R ( ~ k 1, k ). (2–17) Theestimateoftherotation R k 1 k thereforeis ^ R k 1 k = f R ( ^ k 1, k ) Werstprovideresultsforthecasewhentherobotmovesinas traightlinewith constantvelocityandorientation.Theproofofthetheorem isintheAppendix. Theorem2.2. Considerarobotthatmovesona2-Dplaneinastraightlinewi tha constantorientation.Formally,forall k k 1, k =0 and t kk 1, k = r 2 R 2 ,forsomevector r InadditiontoAssumption 2.1 ,assumethattherelativeorientationerror ~ hasapdfthat issymmetricarounditsmean E[ ~ ] ,thetranslationmeasurementerrors ~ t kk 1, k k =1,... arewidesensestationarywith b k = b P k = P ,and k = forall k .Inthatcase,we have E[ e ( n )]= n r ( I c R ) 1 ( I ( c R ) n )( c R r + ) Tr [ Cov ( e ( n ), e ( n )) ] = n + ( n ), (2–18) where c :=E[cos ~ E[ ~ ] ], R := f R (E[ ~ ]), (2–19) andthescalars ( n ) aregivenin ( 2–15 ) 2 Sincether.v. ~ isnotdegeneratebyAssumption 2.1 ,wehavethat j c j < 1 .The spectralradiusof c R isstrictlylowerthanunitysince j c j < 1 and R 2 SO (2) .Hence I c R isinvertibleand ( n ) in( 2–15 )arewelldened. 35

PAGE 36

AnimmediatecorollaryofTheorem 2.2 isthatforstraightlinemotion,boththebias andthevarianceofthepositionestimationerrorgrowasymp toticallylinearlywithtime. Thisfollowsfromtheexpressionsforthebiasandthevarian ceuponusingthefactthat c < 1 .However,duetothepresenceofthe c n terms,thegrowthlookssuperlinearfor intermediatevaluesofthetimeindex n .SimulationspresentedinSection 2.4.2 verify thisstatement;seeinparticularFigure 2-4 and 2-5 .Thelineartrendbecomesvisible onlywhenlargevaluesofthetimeindex n areconsidered.Thismaybeoneofthe reasonsthattheerrorisbelievedtogrowsuper-linearlywi thtimeintheliterature. Thenextcaseisaperiodictrajectoryin2-D.Wesaytherobot movesinaperiodic trajectorywithperiod p iftheabsoluteorientationandpositionoftherobotsatis es thefollowingconditions: 0, k = 0, k + p and t 00, k = t 00, k + p forall k .Theshapeofthe (closed)pathalongwhichtherobotmovescanbearbitrary.I nthestatementofthe theorem, denotesthenumberofperiodsuptotime n ,and q todenotetheresidual, i.e., ( n ):= b n = p c and q := n p Theorem2.3. Considerarobotmovingin R 2 whosetrajectoryisperiodicwithperiod p .InadditiontoAssumption 2.1 .1-Assumption 2.1 .4,assumethattherstandsecond momentsofthemeasurementerrorsareperiodicwithperiod p (sothat b k = b k + p k = k + p and P k = P k + p ).Inthatcase, E[ e ( n )]= t 00, q ( I ( c R ) p ) 1 ( I ( c R ) p ) w ( p ) ( c R ) p w ( q ), (2–20) where w ( j ) isgivenby w ( j ):= j 1 X i =0 ( c R ) i R 0i +1 c R t i +1 i i +1 + i where c R areasdenedinTheorem 2.2 TheproofofthetheoremisprovidedintheAppendix.Theassum ptionofthe moments k etc.beingperiodicwithperiod p ismotivatedbytheuseofvision-based 36

PAGE 37

sensorstomeasurerelativeposes.Inthatcasethemeasurem enterrorstatisticsmay dependonthescenethecamerasees,whichwillrepeatitself every p instantsdueto theperiodicnatureoftherobot'smotion.Notethati.i.d.e rrorsareaspecialcaseof errorswithperiodicstatistics,andsotheresultisalsova lidinsuchascenario. Itcanbeshowninastraightforwardmannerfrom( 2–20 )thatthebiasis O (1) ,by usingthefactthat j c j < 1 .ThisisconsistentwithTheorem 2.1 sincetherobotstaysina boundedregionforalltimewhenfollowingaperiodictrajec tory. 2.3ReducingtheBias Wenowdiscussapossiblewaytoreducethebiasinthepositio nestimatebyusing thelessonslearnedfromtheanalysisthatledtoTheorem 2.1 .Firstofallwenotethat computing E[ ^ t 0k 1, k ] requiresknowledgeoftruerelativerotationsandtranslat ions,and thereforethebiascannotbeeliminatedbysimplycomputing itandsubtractingitfrom theestimatedtranslation ^ t 0k 1, k atevery k .Insteadtheproposedmethodconsistof modifyingtherawmeasurements ^ R k 1 k ^ t kk 1, k intotheso-calledmodiedmeasurements ( ^ R k 1 k ) modif ,( ^ t kk 1, k ) modif ,thataredenedbelow,andthenusingthemintheposition estimation. ( ^ R k 1 k ) modif := ^ R k 1 k ( R ) 1 ( ^ t kk 1, k ) modif := ^ t kk 1, k b where b :=E[ ~ t kk 1, k ], k 1. (2–21) Weassumethatthetranslationmeasurementsarestationary inmeansothat b isa constant.Themodiedmeasurementscanbecomputedfromthe rawmeasurements andknowledgeof R b ,whichcanbedeterminedfromananalysisofsensornoise characteristic.Forinstance,thequestionofestimating b forvision-basedsensorsis examinedin[ 9 10 ].Thepositionattime k isnowcomputedasbefore,butwiththenew correctedmeasurementsinplaceoftherawsensormeasureme nts ^ t kk 1, k and ^ R k 1 k 37

PAGE 38

Specically, ( ^ R 0k ) modif := k Y i =1 ( ^ R i 1 i ) modif ( ^ t 0k 1, k ) modif :=( ^ R 0k ) modif ( ^ t kk 1, k ) modif andnally, ( ^ t 00, n ) modif = n X k =1 ( ^ t 0k 1, k ) modif Therationaleforthisproposalcomesfromthefollowingrel ationshipsthatcanbe shownfromstraightforwardcalculations: E[( ^ R k 1 k ) modif ]= R k 1 k (2–22) E[( ^ R k 1 k ) modif ( ^ t kk 1, k ) modif ]= R k 1 k t kk 1, k (2–23) wherethesecondrelation( 2–23 )holdsiftherawrotationandtranslationmeasurements ^ R k 1 k ^ t kk 1, k areuncorrelated.Themodicationoftherawmeasurementse liminates thegeometricdecayofthelengthoftherelativetranslatio nmeasurementsafterbeing transformedtoframe 0 .AsdiscussedinSection 2.2.2 ,thisdecaywasthemaincauseof thebiasgrowth.If ^ R k 1 k ^ t kk 1, k arecorrelatedbutthemotionislimitedtoa2-Dspace,a slightlydifferentmethodcanbeusedthatensuresthat( 2–22 ),( 2–23 )hold. 2.4SimulationVerication Inthissectionweempiricallyestimatethemeanandcovaria nceoftheestimation errorbyconductingMonte-Carlosimulationsandcompareth emwiththetheoretical predictions.Insection 2.4.1 wesimulatearobotmovingalongarandomlygenerated 3-Dpathandcomparetheresultswiththeupperandlowerboun dspredictedin Theorem 2.1 .Insections 2.4.2 and 2.4.3 wepresentsimulationsforthe2-Dscenario withstraightlineandperiodictrajectoriessothatempiri calresultscanbecomparedwith predictionsofTheorem 2.2 and 2.3 .Ineachcase,therobotissimulatedmovingalong eitherthestraightlineorperiodictrajectoryataspeedof 0.32m = s forabout 5.5 hours, travelingadistanceof 6400 meters.Inallthreesimulations,measurementsoftherobot 's 38

PAGE 39

relativeposeweretakenevery 0.2 seconds.AllsimulationsareconductedinMATLAB c r Totheextentpossible,theparametersusedinthesimulatio nsarethesameasthosein theexperiments.2.4.13-DSimulation Forthe3-Dcasewesimulatearobotmovingalongapaththatis shownin Figure 2-2 .Therobottraversesthispathfromthestartingpointtothe leftandmoving totheright.Measurementerrorsaregeneratedasfollows.T heerrorinrotation ( ~ R k 1 k ) isintroducedbyapplyingarandomunit-quaternionateacht imestepdrawn independentlyfromaVonMises-Fisherdistributionwithco ncentrationparameter k =10000 .Thereaderisreferredto[ 47 ]fordetailsonVonMises-Fisherdistribution. Theerrorsinrelativetranslationateachtimestep ( ~ t kk 1, k ) aredrawnfromazero-mean normalrandomvariablewithcovariancematrix (2.5 10 5 ) I 3 3 .Thecorresponding constantsnecessarytocomputetheupperboundsinTheorem 2.1 areobtainedfrom randomlygeneratedmeasurementstosimulateasensorchara cterizationtestand foundtobe r =0.9997 =0.1295m b=0m =0.008m p =7.45 10 5 m 2 ,and p=7.55 10 5 m 2 Figure 2-3 comparestheempiricallyestimatedbiasandvariancewitht heupper boundsgivenbyTheorem 2.1 .Theempiricalestimatesareobtainedfrom4500Monte Carlosimulations.Aspredictedbythetheorem,thebiasinth epositionestimategrows withoutboundsincetherobot'spositionisgrowing(innorm )withoutbound.Wesee thattheboundspredictedbythetheoremareofthesameorder ofmagnitudeasvalues obtainedempirically.However,theboundsforthevariance areratherloose. 2.4.2Straight-Line2-DTrajectory Forthestraightlinecase,wesimulatearobotmovinginastr aightlineonaplane withaconstantvelocityof [0.2263,0.2263] T m = s andconstantorientation.Twotypesof simulationsareconducted. 39

PAGE 40

0 2000 4000 6000 -50 0 50 -50 0 50 x(m) y(m)z(m) Figure2-2.The3-Dpathusedforthesimulationinsection 2.4.1 .Thereddotindicates therobotsinitiallocation,whiletheredcircleindicates therobotsnal location. 0 2 4 6 8 10 x 10 4 0 1000 2000 3000 4000 5000 6000 Time nk E[ e ( n )] k 2 ( m )Empirical UpperBound LowerBound (a) 0 2 4 6 8 10 x 10 4 0 2 4 6 8 10 12 x 10 6 Time n Empirical UpperBound Tr [ Cov ( e ( n ), e ( n )) ] ( m 2 )(b) Figure2-3.Comparisonoftheoreticalboundsvs.simulatio nresults.Thelabels“Upper Bound”or“LowerBound”indicatethetheoreticalboundsfound in Theorem 2.1 ,whilethelabel“Empirical”indicatesthesample(a)biasor (b) variancefoundusinga 4500 -iterationMonte-Carloexperiment. Inthersttype,whichwecallsimulateddata,noisymeasure mentsofthe rotation,i.e., ^ k 1, k aregeneratedasaLaplacedistributedrandomvariableusin ga pseudo-randomnumbergenerator.ThereasonforchoosingaL aplacedistribution, overthemorecommonlyusedGaussiandistribution,isthefo llowing.Weobtained alargesampleof2Dorientationestimatesfromimagestaken withamachine-vision cameraandperformedhypothesistestingforthreedistribu tions:Laplacian,Gaussian 40

PAGE 41

andFisherVon-Mises.OnlytheLaplacedistributionpassed thetest.Werefrainfrom givingdetailsofthehypothesistestinghere;theycanbeob tainedfromtheauthorsupon request.Noisymeasurementsofthetranslations,i.e., ^ t kk 1, k weregeneratedfromnoisy measurementsoftranslationdirection,whichwecall kk 1, k ,andtranslationmagnitude, whichwecall d k 2 R + ,as ^ t kk 1, k = ^ d k ^ kk 1, k ,where ^ d k and ^ kk 1, k arenoisyestimates of d k and kk 1, k ,respectively.Notethat kk 1, k isaunitnorm2-vector.Thisisdoneto simulaterelativeposemeasurementwithIMU/wheelodometr yandamonocularcamera withoutscaleinformation.Thecameraprovidesrelativetr anslationdirectionbutnotthe magnitudeoftranslation,whichismeasuredbyIMUs/wheele ncoders. Inthesecondtypeofsimulations,whichwerefertoassimula tedcamera,the vision-basedrelativeposeestimationsensorissimulated inamorerealisticfashionby generatingsyntheticimagedata,fromwhichrelativerotat ionanddirectionoftranslation areestimated.Themagnitudeoftranslationmeasurementsa regeneratedasinthe “simulateddata”case. Simulateddata: Ateachtimestep k ,ameasurementoftherelativeorientation isconstructednumericallyas ^ k 1, k =0+ ~ k 1, k ,wheretheorientationerror ~ k 1, k ischosentobea 0 -meanLaplacedistributedr.v.RecallthataLaplacedistri bution with meanandvariance 2 2 hasthepdf f ( ~ )= 1 2 e j ~ j = .Thevalueof chosen is 3.6 10 3 ,whichbesttstheorientationmeasurementerrorstatisti csgenerated bythesyntheticmonocularcamera-basedrelativeposesens orthatisusedinthe experimentspresentedinthesequel.Thenoisymeasurement oftranslationdirection ^ kk 1, k generatedas ^ kk 1, k = 0B@ cos ~ k 1, k sin ~ k 1, k sin ~ k 1, k cos ~ k 1, k 1CA kk 1, k where ~ k 1, k isazero-meanLaplacerandomvariablewithvariance 3.07 10 2 rad 2 and kk 1, k = 1 p 2 [1,1] T isthetruetranslationdirection.Themagnitudeofthetran slation 41

PAGE 42

0 5 10 0 500 1000 1500 2000 2500 3000 replacements Time nk E[ e ( n )] k 2 ( m ) Empirical Theoretical 10 4 (a) 0 2 6 8 10 0 5 10 15 Time n Empirical Theoretical 10 10 4 4 6Tr [ Cov ( e ( n ), e ( n )) ] ( m 2 )(b) Figure2-4.Comparisonoftheoreticalresultsforstraight linevssimulationdata.The predictedvaluefromTheorem 2.2 isgivenbythedashedline,whilethesold lineindicatesthesample(a)biasor(b)variancefoundusin gaMonte-Carlo experimentforthe“simulateddata”case. is d k =6.4 10 2 manditsnoisymeasurementisgeneratedas ^ d k = d k + ~ d k ,where ~ d k isazero-meanGaussianrandomvariablewithmean 0 variance 8.5467 10 5 m 2 .Thesenumbersarechosentobeconsistentwiththoseseenin anexperiment withawheeledrobotdescribedlaterinSection 2.5 .Theparameters b c P that areneededtocomputethepredictionsbyTheorem 2.2 ,areestimatedbyasimulated sensorcharacterizationtest,i.e.,byappropriateaverag ingofrandomlygenerated data.Theyturnouttobe b =[ 0.6842, 0.6842] 10 3 m c =1 1.2873 10 5 Tr [ P ] =1.2479 10 4 m 2 ,and = c b Themeanandcovarianceofthepositionestimationerrorate verytimeinstant areempiricallyestimatedbyaveragingover76,600Monte-C arlosimulations.Figure 2-4 presentstheestimatedmeanandcovariances,andthevalues predictedbyTheorem 2.2 WeseefromthegurethatthepredictionfromTheorem 2.2 matchesestimatesfrom Monte-Carlosimulationsquitecloselyevenforthelargeti meintervalsusedinthe simulations. Simulatedcamera: Wenowsimulatethescenarioinwhichrelativepose measurementsareobtainedbyacalibratedmonocularProsili caEC1020camera 42

PAGE 43

0 5 10 0 1000 2000 3000 4000 Time nk E[ e ( n )] k 2 ( m )Synthetic Theoretical 10 4 (a)Bias 0 5 10 0 0.5 1 1.5 2 Time n Synthetic Theoretical 10 10 4 7Tr [ Cov ( e ( n ), e ( n )) ] ( m 2 )(b)Variance Figure2-5.Comparisonoftheoreticalresultsforstraight linevssimulationcamera.The predictedvaluefromTheorem 2.2 isgivenbythedashedline,whilethesold lineindicatesthesample(a)biasor(b)variancefoundusin gaMonte-Carlo experimentforthe“simulatedcamera”case. andwheelodometersfoundonaPioneerP3-DX.Tosimulateanesti mateofthecamera ego-motionbetweenconsecutivetimesteps,supposebetwee n k and k +1 ,aset of 50 3-Dpointsarerandomlygeneratedinthevolumevisibletoth ecameraattime step k ,withtheircoordinatesrepresentedinthecoordinatefram eattachedtothe cameraattimestep k .Thepointsarethenactedonbythetruetransformationfrom k to k +1 tondthecorrespondingcoordinatesinthecoordinatefram eattachedtothe cameraattimestep k +1 ,UsingacalibrationmatrixcorrespondingtotheProsilica EC1020camera,thepointsareprojectedintotheircorrespon dingimageplane.This formsasetofcorrespondencesanalogoustothefeaturepoin tsextractedfromactual imagepairs.Eachfeaturepointisnowcorruptedbyuniformno isewithsupportlying ina 2 2 pixelsquareaboutthepoint.ARANSAC[ 48 ]assistednormalized8-point algorithm[ 49 ]isusedtoestimatetherotation ^ R andtranslationdirection ^ between thetwotimestepsfromthesepointcorrespondences.Theaxi sofrotationwasthen alignedwiththenormaltotheplaneofmotionandthecompone ntofthetranslation vectorinthatdirectionwasdroppedtoinsurethemotionest imatesremainedinthe plane.Themagnitudeoftranslation ^ d isgeneratedasintheSimulatedDatacase.The 43

PAGE 44

valuesoftheparametersthatareneededtocomputethepredi ctionsbyTheorem 2.2 areestimatedfromasimulatedsensorcharacterizationtes tlikebefore.Thevalues arefoundtobe b =[ 0.5767, 0.5904] 10 5 m Tr [ P ] =1.6382 10 4 m 2 ,and c =1 2.1462 10 5 Figure 2-5 comparesthepredictionsofbiasandvariancebyTheorem 2.2 tothose estimatedfrom 1000 Monte-Carlosimulations.ThenumberofMonte-Carlosimula tions issmallerinthesyntheticdatacaseduetotheprohibitivel yhighcostofconducting thesesimulations.WeseefromFigure 2-5 thatTheorem 2.2 accuratelypredictsthe positionestimationerrorcomputedfromsyntheticimageda ta.Thepredictiontowardthe endofthesimulationtimeisnotasaccurateasinthesimulat eddatacase,whichisdue tothesmallernumberofMonte-Carlotrials.2.4.3PeriodicTrajectory Wenextsimulatearobotmovingonacirclewithcircumferenc eof 4.11 msothat itstrajectoryisperiodicwithperiod p =3020 .Thespeedoftherobotisapproximately 0.32m = s ,sothatittraversesthecircleabout 47 timesbeforecompletingoneperiod. Thetrajectoryischosentobeclosetothatencounteredinan experimentswitha PioneerP3-DXrobot,whichwillbepresentedinSection 2.5 .Noisyrelativepose measurementsaregeneratedasintheSimulatedDatacaseinst raightlinemotion. OrientationmeasurementerrorsareLaplacedistributed(w ithmean E[ ~ ]=6.8 10 5 m andparameter =3.6 10 3 )whiletranslationmeasurementerrorsaregenerated inthesamemanner,andwiththesamedistributionsasintheSi mulatedDatacasein straightlinemotion;withthenewtruevaluesgivenby kk 1, k = [ 0.049,0.999 ] T and d k =0.064m Figure 2-6 showstheempiricalestimatesofbiasandvariancefrom 29,970 Monte-Carlosimulations.Italsopresentsthebiaspredict edfromTheorem 2.3 .We seefromFigure 2-6 (a)thatthebiasisquiteaccuratelypredictedbyTheorem 2.3 .The highfrequencyoscillationcorrespondstothetimeittakes fortherobottotraversethe 44

PAGE 45

Time nk E[ e ( n )] k 2 ( m )9 9.05 10 4 0.47 0.44 0 0 5 10 15 0.2 0.4 0.6 0.8 1 1.2 10 4 Empirical Theoretical (a)Bias 2 6 8 Time n 0 0 5 10 10 15 10 4 4 Tr [ Cov ( e ( n ), e ( n )) ] ( m 2 )Empirical (b)Variance Figure2-6.Comparisonoftheoreticalresultsforperiodic motionvssimulationresults. Theresultsarefora2-Dscenariowithperiodicmotion.Thel abel “Theoretical”indicatespredictionsfromTheorem 2.3 .Thelabel“Empirical” indicatesestimatesofthe(a)biasor(b)variancefoundusi ngaMonte-Carlo experiment. (a) k 1 k 2 k 3 k 4 k 5 k 6 (b) Figure2-7.Therobotusedintheexperiments.In(b)thetraj ectoryoftherobotis indicatedbyshowinganimageoftherobot,asviewedbytheov erhead camera,atmultipletimesteps. circleonce.Thelowerfrequencyoscillationcorrespondst otheperiodofthetrajectory. Thevarianceseemstogrowlinearlywithtime,asonecanseef romFigure 2-6 (b),buta formulaisnotavailableintheperiodiccaseforcomparison 45

PAGE 46

2.5ExperimentalVerication Inthissectionwereportresultsofexperimentsconductedw ithawheeledPioneer P3-DXrobotthatisequippedwithacalibratedmonocularProsi licaEC1020camera andwheelodometers.Theimagescapturedbythecameraareus edtoestimatethe relativerotationanddirectionoftranslation.Thedistan cetravelledisestimatedbythe wheelodometersandthenfusedwiththedirectionoftransla tionestimatedfromthe cameratoestimatethetranslationvector.Therelativepos eofthecameraismeasured every 0.2 seconds.Anoverheadcameraisusedtomeasurethetrue2-Dpos eofthe robot.Duetospaceconstraintsoftheindoortestset-up,th etrajectoryoftherobotwas chosentobeanapproximatelycircularonewithradius 0.65m andonerotationtaking approximately13seconds(seeFigure 2-7 ).Althoughtherobot'strajectoryisnottruly periodic;itisapproximatelyperiodicwithperiod p =3020 (i.e., 604 seconds). 2.5.1TestSet-Up Figure 2-8(b) showsaschematicoftheexperimentalset-up.Theglobalcoo rdinate frameisdenedtocoincidewiththecoordinateframeattach edtoanoverhead cameraviewingtheplaneofmotion.Thatis,theoriginofthe globalcoordinateaxes correspondstothecamera'sfocalpoint.Theoverheadcamer aisusedtoobtainthetrue poseoftherobot.Therobot'slocalcoordinateframewasde nedbyacubeafxedto thetopofthebox.Agridconsistingofsixdotswasplacedato pthecubewithaknown geometry(seeFigure 2-8(a) ),whichallowsreconstructionofthefull3-Dposeofthe robotfromthesinglemonocularcamera.Althoughsomeerrorb etweenthetrueposeof therobotandthatestimatedfromtheoverheadcameraisunav oidable,thiserrordidnot haveanycumulativeeffectovertime.Thereforetheposeest imatedfromtheoverhead cameraistakenasthegroundtruth. TheKLTtracker[ 50 ]wasusedtotrackfeaturepointsacrosspairsofimages,and aRANSAC-assistednormalized8-pointalgorithmwasusedtoes timatetherelative rotationanddirectionoftranslationbetweeneverysucces sivepairsofimages.All 46

PAGE 47

x r y r z r (a)Topview x r y r z r x g y g z g (b)CoordinateAxes Figure2-8.Schematicofthetestset-up.The(a)overheadvie wand(b)sideview, includingoverheadcamera,areshown.Theglobalcoordinat eframeand robotxedcoordinateframearealsoindicated. estimationwasperformedoff-line.EvenwithRANSAC,outliers inpoint-correspondences cancauselargeerrorsintherelativeposeestimates.Anad-h oc“lter”wasimplemented toreducetheeffectofsucherrorsasfollows.Iftheestimat edrelativeposefromthe camerawasdeemedinfeasible(whichwasdeterminedbythekn ownmotionofthe robot),therelativerotationandrelativetranslationdir ectionestimatedintheprevious timestepwasusedastheestimateforthecurrenttimestep.T herelativetranslation betweentwotimeinstantswasestimatedfromtherelativetr anslationdirectionandthe estimateofitsmagnitude,thelatterbeingobtainedfromaw heelodometer.Therelative posessoobtainedwerechainedtogethertoobtainanestimat eoftheglobalposition andorientationoftherobotateverytimestep,asdescribed inSection 2.1 2.5.2TestResults Thepositionestimationerrorateachtimestepiscomputedb ycomparingthe groundtruthwiththerobot'spositionestimatedfromrelat iveposemeasurements.The biasandvarianceinthepositionestimationerroratanygiv entimesteparedetermined byaveragingover17experiments,whereeachexperimentcon sistsoftherobotmoving 47

PAGE 48

0 1000 2000 3000 4000 5000 0 0.2 0.4 0.6 0.8 1 1.2 Timeindex nk E [ e ( n )] k 2 ( m )(a)Bias 0 1000 2000 3000 4000 5000 0 0.5 1 1.5 Timeindex n Tr [ Cov ( e ( n ), e ( n )) ] ( m 2 )(b)Variance Figure2-9.Experimentalresultsforthe2-Dcircularmotion caseusingasingleP3-DX robot. onitspathfor 1000 seconds( 5000 timesteps).Theexperimentallyobtainedbiasand varianceofpositionestimationerrorareshowninFigures 2-9(a) and 2-9(b) Fromtheguresweseethattheexperimentallyobtainedresu lts-especiallythe bias–closelyresemblethoseseeninsimulations(cf.Figur e 2-6(a) 2-6(b) ),which inturnareaccuratelypredictedfromtheanalysis.Theexpe rimentallyobtainedbias staysbounded,asTheorem 2.3 predicts.Thevariancealsoshowsanon-average lineargrowthwithtime,whichisconsistentwithTheorem 2.1 .Theexperimentprovides additionalcondenceinourtheoreticalresults.Inadditi on,wenotethatwhilethe theoreticalpredictionsareforadead-reckoningtypeposi tionestimationalgorithm,the algorithmusedintheexperimentswasmoreakintoakinemati c-modelbasedlter.Still thetheoreticalpredictionsmatchtheexperimentalresult sratherwell.Thisisexpected since-asarguedearlier-theanalysisisapplicabletobroa derclassofestimation algorithms;seethediscussioninSection 2.1 afterEq.( 2–5 ). Thereareneverthelesssomediscrepanciesbetweentheexpe rimentallyobtained biasandvariancevaluesandthoseobtainedfromsimulation s,ascanbeseen comparingFigure 2-6(a) withFigure 2-9(a) andFigure 2-6(b) withFigure 2-9(b) Theseareduetothedifferencesbetweentheexperimentsand simulations.First, theexperimentalbiasandvariancesvaluesarecomputedbya veragingoveronly 48

PAGE 49

0 5 10 15 -10 -8 -6 -4 -2 0 x ( m )y ( m )Figure2-10.Arandomlygeneratedpathin2-Dusedtotestthe bias-reductionmethod. 17experiments,whereasthesimulationestimatesarecompu tedfromatleast1000 Monte-Carlosimulations,insomecasesmanymore.Thereaso nforthissmallernumber ofexperimentaltrialsisthedifcultyandtimeneededinpe rformingtheseexperiments. Thesmallernumberoftrialsavailabletoaverageoverprodu cedlessaccurateestimates. Second,thecharacteristicsofthecameraerrorcouldnotbem odeledinanyofour simulations.Third,itisnotpossibletoensureatrulyperi odictrajectoryinareal experiment.The“high-frequency”oscillationsintheexpe rimentalbiasandvariance plotsareat 7.8 10 2 Hz,whichcorrespondtotheaveragetimetherobottakesto traversethecircleonce.Theseareseeninthesimulationsa swell;seeinparticular theinsetinFigure 2-6(a) .However,theseoscillationsarenotparticularlyvisible inthe variance,onehastomagnifythecurveinFigure 2-6(b) considerablytoseethem.We believethenoticeabledifferenceincaseofthevarianceco mesfromtheverysmall numberofrunsthatweaveragedover. 2.6ReducingtheBias Thebias-reductionmethod,rstpresentedinSection 2.3 wastestedwiththehelp ofsimulationstodetermineitseffectiveness.Thefollowi ngtypesoftrajectoriesin2-D wereusedinthesimulations:(i)straightline(ii)circula r,(iii)randomwalkinacity-like grid,and(iv)arandomlygeneratedsmoothpath.Theperform ancewasseentobe similarinallcases;soonlythedetailsforcase(iv)arando mlygeneratedsmoothpath 49

PAGE 50

0 200 400 600 800 1000 0 5 10 15 20 25 WithAdjustment NoAdjustmentk E[ e ( n )] k (m)Timeindex n (a)Bias 0 200 400 600 800 1000 0 50 100 150 200 250 300 WithAdjustment NoAdjustment Timeindex nTr [ Cov ( e ( n ), e ( n )) ] ( m 2 )(b)Variance Figure2-11.Performanceofthebias-reductionmethod,for thepathshownin Figure 2-10 .Thelegend“WithAdjustment”referstotheestimatesobtaine d withthebias-reductionmethodofSection 2.3 .Thebiasisreducedto almostzerowiththeproposedmethod.Allquantitiesareesti matedfrom morethanamillionMonte-Carlosimulations. ispresented.Thepathourrobottraversedintheexperiment isshowninFigure 2-10 Noiseinthesensormeasurementswassimulatedbyaddingi.i .d.Gaussianrandom vectorswithmean [0.05,0.02] T m andcovariancematrix 0.05 I totherelativetranslation measurementsateachtimestep.Theangledescribingtherel ativerotationbetween eachtimestepwascorruptedbyaddingi.i.d.Gaussianrando mvariableswithmean 6.8 10 3 andvariance 2.6 10 3 .Thesensorcharacteristics R and b neededfor thecorrectionweredetermineda-priori;theirvaluesare R =0.9987 f R (6.8 10 3 ) b =[0.05,0.02] T .Theestimatesofthebiasandvarianceinthepositionestim ateswere obtainedfrommorethanamillionMonte-Carlosimulations. Thecomparisonbetween thebiaswiththemethoddescribedinSection 2.3 andthatforthebaselinecase(no modication)isshowninFigure 2-11 (a).Thecomparisonofthevariancesisshownin Figure 2-11 (b). Weseefromthesimulationsthattheproposedmethodofbiasr eductionsignicantly reducesthebias.Theresultingvarianceisthesameorsmall er,forsmallvaluesof time.Forlargevaluesoftime,theresultingvarianceislar gerthanthatachievedif 50

PAGE 51

measurementswerenotmodied.Thisisexpectedsincethemo dicationsintroduce additionaluncertainty.Inparticular,themodiedrotati onmeasurementsarenolonger elementsof SO ( d ) .Asimilartrendisseenforallothertrajectoriestested:t hebiasis signicantlyreducedforalltime,whilethevarianceiseit hersmalleroralmostthesame forsmallvaluesoftimebutislargerforlargevaluestime. 2.7Summary Weexaminedthegrowthoferrorinpositionestimatesobtain edfromnoisyrelative posemeasurements.Weshowedthatinboth2-Dand3-D,thebia sandthevarianceof thepositionestimationerrorgrowsatmostlinearlywithti meordistancetravelled.The precisegrowthrateofthebiasdependsonthetrajectoryoft herobot.Specically,ifthe robotstaysinaboundedregion,thebiasisupperboundedbya constantforalltime.It wasprovedthatthevariancegrowthrateisalsolowerbounde dbylinearfunctionoftime ifthetranslationmeasurementerrorsarelargeenough.Exac tformulasfortheerrorbias andvariancewereobtainedfortwoparticular2-Dtrajector ies,straightlineandperiodic. ExtensiveMonte-Carlosimulations,andexperimentswithaw heeledrobot,wereusedto verifytheresults. Theresultsofthischaptershowthatlocalizationerrorgro wthrateis,infact,not superlinearwithtimeordistanceevenwithoutabsoluteori entationsensors.Inaddition, itturnsoutthattheasymptoticgrowthrateofthebiasdoesn otchangeevenifallthe measurementsareunbiasedorevenifthetranslationmeasur ementsarecompletely errorfree.Thebiasgrowthisprincipallyduetothefacttha ttheexpectedvalueofthe estimatedpositionconvergestoapoint,irrespectiveofho wtherobotismoving.This occurssince r ,thenormoftheexpectedrotationerror,isstrictlylessth anunity.Asa result,themagnitudeofthemeasuredtranslation,oncethe measurementistransformed totheglobalcoordinateframe,decaysgeometricallywitht ime. Oneimportantassumptionsmadefortheanalysiswasthatthe measurements collectedattwodistincttimeinstantsarestatisticallyi ndependent.Thoughthismaynot 51

PAGE 52

holdinpractice,theresultsobtainedfromexperimentsand simulationswithsynthetic imagedataareconsistentwiththetheoreticalpredictions .Thisshowsthattheanalysis isnotsensitivetotheassumptionsofindependence.Thesuf cientcondition( 2–11 ) forthevariancetobeasymptoticallylinearintimeisnotsa tisedinthesimulations andtheexperiment.However,theempiricallyestimatedvar iancefromsimulationsand experimentseemstogrowlinearlywithtime.Thisindicates thatthesufcientcondition isconservative.Determininganecessaryconditionforvar iancegrowthtobelinearisan openquestion. Amethodtoreducethebiasgrowthratewassuggestedbythele ssonslearnedin theanalysisoferrorgrowth.Simulationsshowedthatthepro posedmethodreduces thebiassignicantlyforalltime,whilehavingnegligible effectonthevarianceforsmall valuesoftime.Themethodcanthereforebepotentiallyused toimprovelocalization accuracyforshortperiodsoftime.Thereareseveralissues thatstillneedtobe addressed.Themethodwasobservedtomakethevariancewors eforlargetime. Soanimportantresearchquestionistodeterminethetimeper ioduptowhichthe methodcanbeused.Themethodrequiresknowledgeofsensorc haracteristics.Its robustnesstoimpreciseknowledgeofsensorcharacteristi cs,andtotimevariationsin thosecharacteristics,alsoneedstobestudied. 52

PAGE 53

CHAPTER3 DISTRIBUTEDCOLLABORATIVE3DPOSEESTIMATION Inthischapterweproposeamethodforfusingmeasurementso fvarioustypesto performcollaborativelocalizationthatimproveslocaliz ationaccuracyoverdead-reckoning. Theproblemisconsideredinsuchafashionastonotrequirea nyparameterizationof thespaceofrotations.Wewillrefertotheproposedmethoda stheRiemannianPose GraphOptimization(RPGO)algorithm. Therestofthechapterisorganizedasfollows.Theproblemi sstatedinSection 3.1 AcentralizedestimationschemeisdescribedinSection 3.2 ,andadistributedalgorithm thatisinspiredbythecentralizedschemeisdescribedinSec tion 3.3 .Simulationand experimentalresultswiththeproposedalgorithmsarepres ented,inSections 3.4 and 3.5 ,respectively.ThechapterconcludeswithadiscussioninSe ction 3.6 3.1ProblemStatement 3.1.1TheCollaborativeLocalizationProblem Consideragroupof r mobilerobotsindexedby i =1,..., r .Timeismeasured byadiscretecounter k =0,1,2,... .Eachrobotisequippedwithalocal,rigidly attachedframeofreference,thatis,acoordinatesystemde nedintherobot'slocal referenceframe.Wecalltheframeofreferenceattachedtor obot i attime k frame i ( k ) .Betweenanytwoframesofreference,sayframe u andframe v ,wedenotethe Euclideantransformationfrom v to u by T uv ,where T uv isanelementofthespecial Euclideangroup SE (3) .Specically,if p u isapointexpressedinframe u and p v isthe samepointexpressedinframe v ,then p u = T uv p v .Wecall T uv therelativeposeof frame v withrespecttoframe u Letframe 0 denotesomexedframeofreferencethatiscommontoallrobo ts. Theabsoluteposeofframe u isthengivenbythetransformation T 0 u .Wewilloften denotetheabsoluteposesimplyas T u .Robot i issaidtobelocalizedattime k whenan 53

PAGE 54

estimateisknownfortheabsoluteposeofframe i ( k ) .Wedenotesuchanestimateby ^ T i ( k ) Measurementsofarobot'sabsolutepose,perhapsfrommeasu rementsfound usingGPSandcompass,areeithernotavailableoronlyrarely available.Instead,each robotisequippedwithproprioceptivesensorssuchthat,at everytime k ,therobotis abletoobtainarelativeposemeasurementwithrespecttoit spreviouspose,givenby ^ T i ( k 1) i ( k ) .Werefertothesemeasurementsas inter-timerelativeposemeasurements Suchmeasurementscanbeobtainedwithinertialorvisionbas edsensors.Additionally, theyneednotbeobtainedfromasensoralone.Instead,ameas urementcouldalsobe theestimatecomputedbyfusingsensormeasurementswithpr edictionsoftherobot's motionfromadynamic/kinematicmodel. Inadditiontoproprioceptivesensors,eachrobotisequipp edwithexteroceptive sensorssothatoccasionally,arobot i isabletoobtainarelativemeasurementofoneor moreotherrobots.Wecallthese inter-robotrelativemeasurements .Ifarobot i collects ameasurementofrobot j attime k ,itcanbeoneofthefollowing: Relativepose :TheEuclideantransformationfromframe j ( k ) toframe i ( k ) ; denotedbythesymbol T Relativeorientation :Theelementof SO (3) thatdescribesthechangein orientationfromframe j ( k ) toframe i ( k ) ;denotedbythesymbol R Relativeposition :Thevectorin R 3 thatdescribesthechangeinpositionbetween theframes i ( k ) and j ( k ) ,expressedinframe i ( k ) ;denotedbythesymbol t Relativebearing :Thevectorofunitlengththatpointsfromframe i ( k ) toframe j ( k ) ,expressedinframe i ( k ) ;denotedbythesymbol Relativedistance :Thedistancebetweenframe i ( k ) andframe j ( k ) ;denotedby thesymbol Whichpairsofrobotswillbeabletoobtainaninter-robotrel ativemeasurementwill dependonmanyfactors,includingthekindofsensorstheyha veon-board,therangeof sensors,etcandsothesetofinter-robotmeasurementsavai lablevarieswithtime.An 54

PAGE 55

1 2 3 4 7 10 5 8 11 6 9 12 0 12 3 0 time Robot1Robot2Robot3 T T T T T T T T T T T T T R t t t Figure3-1.Thegraphcorrespondingtoagroupof 3 robotsattime k =3 .Each(robot, time)pairislabeledwiththecorrespondingnodeindexfrom V 0 (3) .Arrows indicateedgesin E (3) ,i.e.,relativemeasurements.Eachedgeislabeledto indicatethetypeofmeasurement.EachrobothadGPSandcompas s measurementsattheinitialtime k =0 .Thereafter,nootherGPSor compassmeasurementswereavailable. implicitassumptionhereisthatarobotisabletouniquelyi dentifyanotherrobotofwhich itobtainsarelativemeasurement,sothatthereisnoambigu ityonwhichpairofrobotsa relativemeasurementcorrespondsto. Thecollaborativelocalizationproblemistheproblemofes timatingtheposeofevery robotatthecurrenttime k withrespecttothecommonframeofreferencebyutilizingth e inter-timeandinter-robotmeasurementscollecteduptoti me k Thesituationaboveisbestdescribedintermsofadirected, time-varying, fully-labeledgraph G ( k )=( V 0 ( k ), E ( k ), ` ( k )) ,wherenodes V 0 ( k ) correspond tovariablesandedges E ( k ) tomeasurements,thatshowshowthenoisyrelative measurementsrelatetotheabsoluteposeofeachrobotateve rytimestep.Thegraphis denedasfollows.Foreachrobot i 2f 1,..., r g andeachtime t k ,auniqueindex(call it u )isassignedtothepair ( i t ) .Howthisindexingisdoneisimmaterial.Theindices f 1,..., rk g deneaset V ( k ) thatisasubsetofthenodeset V 0 ( k ) ofthegraph.Werefer totheframeofreferenceattachedtorobot i attime t asframe u ,where u isthenode assignedtothepair ( i t ) .Weintroduceanothernode,denotedby 0 ,thatcorresponds tothecommonframeofreferenceinwhicheveryrobot'sposei stobedetermined. 55

PAGE 56

Thenode 0 iscalledthegroundednode.Thenodesetofthegraphisthend enedas V 0 ( k ):= V ( k ) [f 0 g .Therelativeposeofframe u withrespecttoframe 0 isdenoted simplyby T u .Wecalltheposes f T u g u 2 V ( k ) thenodevariablesof G ( k ) .Eachrelative pose(eithermeasurementornodevariable) T canberepresentedbyaorientationand positionpair, R t .Wewillthereforefreelyreferto T u anditscorrespondingorientation R u orposition t u componentsasnodevariables. Thesetofdirectededgesattime k ,denoted E ( k ) ,correspondstothenoisy inter-timeandinter-robotmeasurementscollecteduptoti me k .Thatis,suppose robot i isabletomeasurerobot j 'srelativeposeattime k ,andlet u v bethenodes correspondingtorobots i j attime k ,respectively.Thenforall k k ,thereexistsa directededge e 2 E ( k ) correspondingtothismeasurement.Sincerobot i measures robot j ,theedge e leavesnode u andarrivesat v ,wedenotethisby e ( u v ) .Similarly, eachinter-timerelativeposemeasurementsofarobotalsoc reatesanedgeinthe graph.Todelineatethetypeofmeasurement,alabelfromthe set f T (pose), R (orientation), t (position), (bearing), (distance) g isattachedtoeachedge.The mapfromthesetofedgestothesetoflabelsisdenotedby ` ( k ) .Foreachedge e 2 E ( k ) where e ( u v ) ,if ` ( k )( e )= s thatindicatesthereisameasurementof type s forframe v withrespecttoframe u ,where s 2f T R t g .Thenoisyrelative measurementassociatedwithedge e ( u v ) isdenotedby ^ T uv ^ R uv ^ t uv ,^ uv or ^ uv for ` ( k )( e )= T (pose) R (orientation) t (position) (bearing),or (distance), respectively.Ameasurementoftype T isreallytwomeasurements,oneoftype R and t .Westillusethenomenclature“measurementoftype T ”foreaseofcomparisonwith priorwork,sincerelativeposemeasurementsarecommonlyc onsideredinexisting literature. IftheabsoluteposeinglobalGPScoordinatesofatleastoner obotisknownat time 0 throughtheuseofaGPSandcompass,thennode 0 canbeassociatedwith aTerrestrialReferenceFrame.Whensuchmeasurementsareno tavailable,node 0 56

PAGE 57

couldcorrespondtotheinitialframeofreferenceofoneoft herobots.Ineithercase, estimatingthenodevariablesisequivalenttodetermining therobots'poseswith respecttoframeofthegroundednode 0 .Withoutthisgroundednode,theproblem oflocalizationfromrelativemeasurementsisindetermina teuptoarotationanda translation. Thegraph G ( k ) iscalledthemeasurementgraphattime k .Figure 3-1 showsan exampleofthegraphcorrespondingtothemeasurementscoll ectedby 3 robotsupto timeindex 3 .Becauseeachrobotmaybeequippedwithmorethanonesensor, multiple distinctedgesmayexistbetweenapairofnodes. Toensureatleastoneestimateexistsforeveryrobotateach time k ,wemakethe followingassumption.Assumption3.1. Eachrobothasaccesstoanestimateofitsabsoluteposeattim e 0 DuetoAssumption 3.1 ,anestimateoftheposeofrobot i attimetime k (equivalently, thenodevariable T u ,wherenode u correspondstothepair ( i k ) )canbecomputedby composingtheinter-timerelativeposemeasurementsobtai nedbytherobot i upto time k .Thisestimateisequivalenttorobot i performingdead-reckoning.Inpractice, Assumption 3.1 holdsifallrobotshaveaGPSandcompassmeasurementattime 0 .IfnoGPSmeasurementisavailable,buteachrobotcanobtain arelativepose measurementwithrespectto,say,robot 1 ,thenagaintheassumptionholds. Oftenmanymoreedgesarepresentinthegraph G ( k ) thanthosenecessaryto formasingleestimateofthenodevariables;robotscanbene tfromcollaborative localizationinsuchascenario.Asanillustrativeexample, considertheexampleshown inFigure 3-1 .Thepath (0,1,4,7,10) providesthedead-reckoningestimateofrobot1 attime 3 ,orequivalentlyanestimateof T 10 .Similarly,thepath (0,2,5,8,11) provides asestimateforthenodevariable T 11 .Additionally,thethreeedgesbetweennodes 10 and 11 correspondingtorelativeorientation,position,anddist ancemeasurementsall provideadditionalinformationabouthow T 10 and T 11 relate.Becausethereisnoise 57

PAGE 58

ineachrelativemeasurement,byincorporatingtheinforma tionfoundinthesethree edges,weexpecttogetabetterestimateofboth T 10 and T 11 .Thegoalofcollaborative localizationistoutilizealledgesinthegraph G ( k ) toimproveoveranyestimatesofthe nodevariablesthatareobtainedusingonlyasinglepath.Whe nthemeasurements arelinearlyrelatedtothenodevariables,thiscanbeaccom plishedbyusingtheBest LinearUnbiasedEstimator,asdonein[ 15 51 ].Inourcase,therelationshipbetweenthe measurementsandnodevariablesisnonlinear.Remark3.1. ThediscussionabovealsoindicatesthatAssumption 3.1 isnotnecessary, butmerelysufcientforlocalization.Forrobot i tobelocalized,allthatisneededis thatthereexistsatime k sothatthereisanundirectedpathfromnode u (where u correspondsto ( i k ) )tonode 0 suchthattheedgesalongthispathareoftype T ,that is,correspondtomeasurementsoftherelativepose.Inthat case,anestimateof t u can beobtainedbyconcatenatingtherelativeposemeasurement salongthepathfrom 0 to u ,andhencerobot i islocalizedattime k .Afterthat,evenifnointer-robotrelative measurementsareavailable,robot i canperformdeadreckoning.Assumption 3.1 is takeninordertosimplifythepresentation.3.1.2TheDistributedCollaborativeLocalizationProblem Theproblemstatedabovedoesnotputanyrestrictiononthea ccesstothe measurementscollecteduptotime k .Inparticular,amethodthatassumesthatall measurementscollectedbyalltherobotsuptotime k areinstantlyavailabletoacentral computerwithnocomputationormemoryconstraintsisallow ed.However,whena largenumberofrobotsareinvolved,withcommunicationacc omplishedthroughtheuse oflow-bandwidthwirelesslinkswithlimitedrange,suchac entralizedschemeisnot feasible.Inaddition,retainingpastmeasurementsinden itelywillquicklyexhaustboth availablememoryandavailableprocessingcapabilitiesof acentralizedcomputingunit. Wenowmodifytheproblembyincludingconstraintsoncommun icationand computation.Inparticular,arobotisnowrequiredtolocal izeitselfbyusinginformation 58

PAGE 59

thatisavailablefromon-boardsensorsanddataitcancolle ctfromitsneighbors.Two robots i and j aresaidtobeneighborsattime k iftheycancommunicateattime k Wenowposethedistributedcollaborativelocalizationpro blemasfollows:each robotistolocalizeitselfbyusingmeasurementscollected byon-boardsensors andinformationitcanobtainbycommunicatingwithitsneig hbors.Tosimplifythe developmentofthedistributedalgorithm,thefollowingas sumptionismade: Assumption3.2. Ifrobot i canobtainarelativemeasurementofarobot j attime k then i and j cancommunicateatthattime. Thoughthisassumptionmayseemstrict,itcan,infact,alwa ysbesatised:robot i simplydropsanymeasurementsinvolving j ifitcannotcommunicatewith j 3.2CentralizedCollaborativeLocalizationAlgorithm Inthissectionwepresentasolutiontothecollaborativelo calizationproblemwhere alltherelativemeasurementsareinstantlyavailabletoac entralprocessorateach time k .Thecentralizedsolutionnaturallyleadstoadistributed scheme,whichwillbe describedinthenextsection. Foreaseofexposition,werstconsiderthecasewheneveryi nter-robotrelative measurementisameasurementoftherelativepose.Thealgor ithmisdescribedin Section 3.2.1 .TheninSection 3.2.2 weexpandouralgorithmtoconsidermeasurements oftherelativepose,orientation,position,bearing,ordi stancebetweennodes. 3.2.1CaseA:HomogeneousMeasurements(RelativePose) Insteadofaddressingtheproblemofestimatingtherobots' currentposesattime k weexaminethemoregeneralproblemofestimatingallthenod evariables T u u 2 V ( k ) of themeasurementgraph G ( k ) ,usingthenoisyrelativemeasurements, ^ T uv ( u v ) 2 E ( k ) Weposethecollaborativelocalizationproblemasanoptimi zationofacostfunction overthesetofnodevariables,wherethecostfunctionmeasu reshowwellagivenset ofabsoluteposesexplainsthenoisyrelativemeasurements collecteduptotime k .The 59

PAGE 60

initialconditionforeachnodevariable T u u 2 V ( k ) isgivenbythedeadreckoning estimate,theexistenceofwhichisassuredbyAssumption 3.1 Toderiveasuitablecostfunction,webreakeachpose(bothn oisyrelativepose measurementsandnodevariables)intoitscorrespondingro tation R 2 SO (3) and translation t 2 R 3 .Inthiswork,arotation R 2 SO (3) isconsideredtobeanabstract operatorandnotnecessarilyequatedtoitsmatrixrepresen tation.Whentherelative posemeasurementsarecompletelyerrorfree, ^ R uv isthetruerelativerotationofframe v withrespecttoframe u .Thisrotationcanalsobeexpressedas R Tu R v ,where R Tu is theadjointoftheoperator R u .Thus,ifnonoisewerepresent, ^ R uv wouldbeequalto R Tu R v .Similarly,intheabsenceofnoise, ^ t uv and R Tu ( t v t u ) wouldbeequal,sinceboth aretherelativetranslationofframe v withrespecttoframe u .Sincenoiseispresent inthemeasurements,howmuch ^ R uv differsfrom R Tu R v (and ^ t uv from R Tu ( t v t u ) )measuredbyasuitabledistancefunction-providesameasur eofhowagivensetof nodevariablesexplainsthenoisymeasurements.Distanceb etweentwotranslation, aselementsof R 3 ,isgivenbythe2-normofthedifference.Tomeasurethedist ance between p q 2 SO (3) ,weuseaRiemanniandistance d ( p q ) : d ( p q )= r 1 2 Tr log 2 ( p 1 q ) (3–1) MoredetailsonthisdistancefunctioncanbefoundinAppendi x B.1 .Thecostfunctionat eachtime k ischosenasasumofedge-costsoveralledges(measurements ): f ( T u u 2 V ( k ) ):= X ( u v ) / e 2 E ( k ) c e ( R u t u R v t v ), (3–2) where c e ( R u t u R v t v ):= 1 2 d 2 ( ^ R uv R Tu R v )+ k ^ t uv R Tu ( t v t u ) k 2 (3–3) where kk denotestheEuclidean2-norm. Iftherelativeposemeasurementswerecompletelyerrorfre e,theminimumvalue ofthecostfunctionwouldbe 0 .Byminimizingthecostfunction,weexpecttondan improvedestimatefortheabsoluteposeofeachrobotoverwh atcanbefoundthrough 60

PAGE 61

deadreckoningalone.Thecostfunctionin( 3–3 )issimilartooneproposedin[ 52 ]fora staticcameranetwork;herethecostfunctionchangeswitht ime. Findingtheminimumofafunctiondenedoveravectorspaceh asbeenstudied extensively.Howeverthefunction f ( ) in( 3–3 )isdenedonacurvedsurface, specically,theproductRiemannianManifold ( SO (3) R 3 ) n ( k ) where n ( k )= j V ( k ) j ,thecardinalityoftheset V ( k ) .Oneoptionforthisoptimizationistousea parameterizationoftherotationsusing,say, 3 3 rotationmatricesorunitquaternions, andthenembeddingthemanifoldinanvectorspaceofhigherd imension.Optimization techniquesapplicabletovectorspacescanthenbeused,wit htheconstraintson theparameterizationofrotationsappearingasLagrangemu ltipliers.Thishowever, leadstoanincreaseinthedimensionalityoftheoptimizati onproblem.Evenwhena parameterizationischosenthatdoesn'tleadtoanincrease indimensionality,such asEulerangles,HopfCoordinates,ortheaxis-anglereprese ntation,theoptimization stepstillrequiresmorecomputationthanwhatispossiblew henthegeometryof SO (3) isutilized.Additionally,suchparameterizationshavepro blemssuchasfailingtobe bijectiveincertainregions;see[ 53 ]foradiscussionoftherelevantparameterizations of SO (3) andtheirassociatedproblems.Instead,ourgoalistondap rovablycorrect algorithmthatutilizesthegeometryofthespacewithoutre lyingonanyparticular parameterization.Weaccomplishthisthroughuseofagradi entdescentalgorithmon theproductmanifold. GradientdescentinaRiemannianmanifoldisanalogoustogr adientdescentina vectorspaceinthefollowingsense.Givenasmoothrealvalu edfunction f denedon amanifold M ,thegradientof f at p 2 M ,denoted gradf ( p ) ,isavectorinthetangent spaceof M at p ,whichisdenotedby T p M .JustasinEuclideanspace, gradf ( p ) points inthedirectionofgreatestrateofincreaseof f .Anexplicitexpressionforthegradientof thecostfunction( 3–3 )isprovidedinthenexttheorem;theproofisinAppendix B.3 61

PAGE 62

Theorem3.1. Thegradientofthecostfunctionshownin ( 3–3 ) at p =( R 1 t 1 ,..., R n ( k ) t n ( k ) ) 2 SO (3) R 3 n ( k ) is gradf ( p )= gradf ( R 1 ), gradf ( t 1 ),..., gradf ( R n ( k ) ), gradf ( t n ( k ) ) where,for u =1,..., n ( k ) gradf ( R u )= R u X ( u v ) 2 E ( k ) h R Tu ( t v t u ) ^ t Tuv ^ t uv ( t v t u ) T R u +log( R Tu R v ^ R Tuv ) i + X ( v u ) 2 E ( k ) log( R Tu R v ^ R vu ) gradf ( t u )= X ( u v ) 2 E ( k ) t u + R u ^ t uv t v + X ( v u ) 2 E ( k ) t u R v ^ t vu t v Minimizingafunction f usinggradientdescentrequiresthecurrentestimateto beupdatedduringeachiterationbymovinginthedirectiono fthenegativegradient. Inavectorspacethisisaccomplishedbysimplysubtracting gradf fromthecurrent estimateforsomeappropriatescalar .OnaRiemannianmanifold,movinginthe directionof gradf isaccomplishedthroughparalleltransport.Theparallelt ransport mapatapoint p :=( R 1 t 1 ,..., R n ( k ) t n ( k ) ) 2 ( SO (3) R 3 ) n ( k ) (3–4) denotedby exp p ,isgivenby exp p ( )= R 1 exp( R T1 R 1 ), t 1 + t 1 ,..., R n ( k ) exp( R Tn ( k ) R n ( k ) ), t n ( k ) + t n ( k ) (3–5) where =( R 1 t 1 ,..., R n ( k ) t n ( k ) ) isanelementofthetangentspace T p ( SO (3) R 3 ) n ( k ) = T R 1 SO (3) T t n ( k ) R 3 ,andthe exp( ) functionappearingintheright handsideof( 3–5 )isthemap exp: L ( R 3 ) L ( R 3 ) denedby exp ( X )= P 1k =0 X k k for x 2 L ( R 3 ) .Thederivationof( 3–5 )isprovidedinAppendix B.2 .Thegradientdescent 62

PAGE 63

lawis p t +1 =exp p t ( t gradf ( p t )), t =0,1,..., where t 0 isthestepsizeforiteration t .Theparameter t ischosenasthe Armijo stepsize ( A ) t = N t ,where N t isthesmallestnonnegativeintegersuchthat f ( p t ) f ( exp p t ( N t gradf ( p t ))) N t k gradf ( p t ) k (3–6) forscalartuningparameters > 0 2 (0,1) .Thenorm kk denedonthevector space T p ( SO (3) R 3 ) n isgivenby k ( R 1 t 1 ,..., R n ( k ) t n ( k ) ) k 2 = n ( k ) X u =1 1 2 Tr T R u R u + T t u t u whichcomesfromthe RiemannianMetric .MoredetailsontheRiemannianmetriccan befoundinAppendix B.1 .Theorem 4.3.1 in[ 54 ]granteesthattheiterates p t converges toacriticalpointofthecostfunction f denedin( 3–7 )as t !1 3.2.2CaseB:HeterogeneousMeasurements Wenowextendourconsiderationtothecasewhentheinter-ro botnoisyrelative measurementscanbeanycombinationofrelativepose,orien tation,position,bearing, anddistance.Theinter-timerelativemeasurementsaresti llofrelativepose.To accommodateheterogeneousmeasurementtypes,wemodifyth ecostfunction( 3–3 )so thatitmeasureshowwellagivensetofnodevariablesteach ofthemeasurements. Thenewcostfunctionis: f ( T u u 2 V ( k ) ):= X ( u v ) / e 2 E ( k ) c e ( R u t u R v t v ), (3–7) 63

PAGE 64

where c e ( R u t u R v t v ) isthecostforedge e ( u v ) thatisgivenby c e ( R u t u R v t v ):= 1 2 8>>>>>>>>>>>>><>>>>>>>>>>>>>: d 2 ( ^ R uv R Tu R v )+ k ^ t uv R Tu ( t v t u ) k 2 if ` ( k )( e )= T d 2 ( ^ R uv R Tu R v ) if ` ( k )( e )= R k ^ t uv R Tu ( t v t u ) k 2 if ` ( k )( e )= t k ^ uv k t v t u k R Tu ( t v t u ) k 2 if ` ( k )( e )= k ^ uv k t v t u k k 2 if ` ( k )( e )= (3–8) Thecostassociatedwithfullpose,orientation,andpositi onmeasurementshave beendiscussedinSection 3.2.1 .Thecostforedgesassociatedwithnoisyrelative bearingmeasurementsismotivatedbythefactthatwhennono iseispresentinthe measurements, ^ uv k t v t u k istheunitvectorpointingfromframe u toframe v expressed inframe u ,asis R Tu ( t v t u ) .Similarly,whennonoiseispresent, ^ uv isthedistance betweenframe u andframe v ,asis k t v t u k .Inthepresenceofnoise,howmuch ^ uv k t v t u k differsfrom R Tu ( t v t u ) (and ^ uv from k t v t u k )providesameasureofhow agivensetofnodevariablesexplainsthenoisymeasurement s. Weagainminimizethecostfunction( 3–7 )throughtheuseofagradientdescent algorithm.Thegradientof( 3–7 )canbecomputedusingtechniquesverysimilarto thoseusedtocomputethegradientof( 3–3 ).Thegradientof( 3–7 )isprovidedin Appendix B.4 .Asinthepreviouscase,wetakethedead-reckoningestimate asthe initialguessforeachnodevariable T u u 2 V ( k ) .Thepseudo-codeofthegradient descentalgorithmonthemanifold ( SO (3) R 3 ) n ( k ) isgiveninAlgorithm 1 ,where > 0 isauser-speciedaccuracythreshold.CorrectnessoftheAl gorithm 1 (convergencetoa criticalpointofthecostfunction f in( 3–7 ))followsfrom[ 54 ,Theorem 4.3.1 ]. Itshouldbenotedthatthealgorithmpresentedaboveisinde pendentofthe parameterizationusedtorepresentrotations.Onecouldus eunitquaternions, 3 3 rotationmatrices,etc. 64

PAGE 65

Algorithm1: RiemannianPoseGraphOptimization Input : G ( k ) ,allnoisymeasurementson E ( k ) ,aninitialguessforeachnode variable ( R u t u ) u 2 V ( k ) Output : n ( ^ R u ^ t u ) o u 2 V ( k ) ^ p initialguess( p denedin( 3–4 )); repeat foreach u 2 V ( k ) do Compute gradf ( ^ R u ) and gradf ( ^ t u ) forthecost f in( 3–7 )(asshownin Appendix B.4 ); endDetermine ( A ) ,theArmijostepsizefrom( 3–6 )(with ^ p for p t ); foreach u 2 V ( k ) do ^ R u ^ R u exp ( A ) ^ R Tu gradf ( ^ R u ) ; ^ t u ^ t u ( A ) gradf ( ^ t u ) ; end until k gradf n ( ^ R u ^ t u ) o u 2 V ( k ) k >" ; 3.3DistributedAlgorithm Inthissectionweproposeanalgorithmforsolvingthedistr ibutedlocalization problem.Thealgorithmrequireslimitedmemory,processor power,andcommunication bandwidthforitsexecution. Foreachrobot i ,let N (+) i ( k ) denotethesetofallrobots j 2f 1,..., r g suchthat,at time k ,robot i canobtainarelativemeasurementwithrespectto j .Similarly,let N ( ) i ( k ) denotethesetofallrobots j 2f 1,..., r g suchthat,attime k ,robot j canobtainarelative measurementwithrespectto i .Theneighborsofrobot i attime k arethengivenbythe set N i ( k )= N (+) i ( k ) [ N ( ) i ( k ) .DuetoAssumption 3.2 ,robot i cancommunicatewithits neighbors N i ( k ) duringtime k Considerthelocalmeasurementgraph G i ( k )= ( V i ( k ), E i ( k ), ` i ( k ) ) ofrobot i whosenodesetissimplytheneighborsof i attime k alongwiththegroundednode 0 and i itself: V i ( k )= N i ( k ) [f 0, i g .Theedgesof G i ( k ) correspondtotheinter-robot measurementsattime k between i anditsneighbors,alongwithanedge e (0, j ) for each j 2 V i ( k ) (seeFigure 3-2 foranexample).Eachnodeinthelocalmeasurement 65

PAGE 66

graph G i ( k ) isassociatedwithanabsoluteposeofarobotattime k .Nopastposes belongtothisgraph.Anedge ( p q ) / e 2G i ( k ) (where i = p or q )correspondsto aninter-robotrelativemeasurementbetweenrobots p and q attime k .Theadditional edges e (0, j ) j 2 V i ( k ) correspondtothe“initial”estimateofrobot j 'sabsolute poseattime k ,denoted ^ T 0 j ( k ) .Eachrobot j obtainstheestimate ^ T 0 j ( k ) attime k byconcatenatingitsposeestimateobtainedattime k 1 withthenoisyinter-time relativeposemeasurementdescribingitsmotionfrom k 1 to k .Theestimate ^ T 0 j ( k ) isthenusedasthemeasurementassociatedwithedge e (0, j ) andbroadcasttoeach ofthatrobotsneighborsforinclusionintheirlocalmeasur ementgraphs.Thegraph G i ( k ) isanowameasurementgraphsinceeachedgehasanassociated noisyrelative measurement.Theedges (0, j ) / e 2 E i ( k ) for j 2 V i ( k ) ensuresthatAssumption 3.1 is satisedforthelocalmeasurementgraph G i ( k ) foreach i 1 2 0 T T T R (a) 3 0 T (b) Figure3-2.ThelocalmeasurementgraphsfortherobotsofFi gure 3-1 :(a) G 1 (3) and G 2 (3) (inthisexampletheyarethesame)and(b) G 3 (3) Thedistributedalgorithmworksasfollows.Ateachtime k ,everyrobot i 2f 1,..., r g formsaninitialestimate ^ T 0 i ( k ) ofitsabsoluteposeasdescribedaboveandobtains inter-robotrelativemeasurementsofeachofitsneighbors j 2 N (+) i ( k ) .Robot i then transmitstoeach j 2 V i ( k ) itsinitialestimateofitsabsolutepose ^ T 0 i ( k ) ,alongwith allinter-robotrelativemeasurementsbetweenitselfand j thatitobtainedattime k Robot i receivesinturnrobot j 'sestimateofitscurrentabsolutepose ^ T 0 j ( k ) ,alongwith relativemeasurementsinvolvingitselfthat j collectedatthattime.Robot i thenexecutes Algorithm1onthelocalmeasurementgraph G i ( k ) .Theunknownnodevariablesinthis 66

PAGE 67

graphconsistofitsownposeandtheposesofitsneighbors(a llattime k ).Afterthe computation,onlytheestimateof i 'sowncurrentposeisretainedinitslocalmemory; allothercomputedvaluesarediscarded.Sincethedistribut edalgorithmissimply Algorithm1appliedtoalocalmeasurementgraph,itinherits thecorrectnesspropertyof Algorithm1.Notethatifrobot i hasnoneighborsattime k ,thedistributedcollaborative localizationalgorithmisequivalenttoperformingself-l ocalizationfrominter-timerelative measurements. Thelocalmeasurementgraphs G 1 (3) G 2 (3) and G 3 (3) correspondingtothe exampleofFigure 3-1 areshowninFigure 3-2 .Attime k =3 ,robot3canseenoother robots,soitwillupdateitsabsoluteposeusingtheinter-t imerelativeposemeasurement alone,withouttheaidofanyinter-robotrelativemeasurem ents.Robots2and1,in contrast,willusetherelativemeasurementsbetweenthemo btainedattime k =3 to updatetheirposeestimates. Notethatthistreatmentofthedistributedalgorithmisasp ecialcaseofthesliding windowapproximationwhenthelengthofthewindowissetto 0 .Forthegeneralsliding windowapproximation,seeSection 6.3 3.4SimulationResults Inthissectionwepresentsimulationscomparingthecentra lizedanddistributed algorithmsandstudytheeffectofcollaborationonlocaliz ationaccuracy.First,in Section 3.4.1 ,weconsiderthecasethatallinter-robotrelativemeasure mentsareof therelativepose.Weexaminethedifferenceinlocalizatio naccuracybetweenthe centralizedanddistributedalgorithms,aswellastheeffe ctofincreasingnumberof robotsonlocalizationaccuracy.TheninSection 3.4.2 weconsidertheheterogeneous measurementcaseinwhichtheinter-timerelativemeasurem entsareofrelativepose, buttheinter-robotrelativemeasurementsmaybeoftherela tivepose,orientation, position,bearing,ordistance.Weexaminetheeffectsthes evariousmeasurementtypes haveontheaccuracyofthelocationestimates. 67

PAGE 68

Thefollowingdenitionswillbeofusewhenevaluatingthee ffectivenessofthe variousalgorithms.Givenasetofrobots,thepositionesti mationerrorofrobot i is denedas e i ( k ):= ^ t i ( k ) t i ( k ) ,where t i ( k ) isitsabsolutepositionat k and ^ t i ( k ) is theestimate.Thebiasinthepositionestimationerrorofro bot i isdenedas k E[ e i ( k )] k where kk isthe2-normand E[ ] denotesexpectation.Thestandarddeviationis denedas p Tr [ Cov ( e i ( k ), e i ( k )) ] ,where Cov ( ) standsforcovariance.Similarly,the orientation(estimation)errorofrobot i isdenedasthescalar e Ri ( k ):= d ^ R i R ( k ) where ^ R i ( k ), R i ( k ) aretheestimatedandtrueorientationsofrobot i at k and d ( ) is thedistancefunctionon SO (3) ,denedin 3–1 .Thebiasandstandarddeviationinthe orientationestimationerroraredenedintheusualway.In eachscenariodescribed below,thebiasandvarianceinpositionestimationerroris estimatedthroughtheuseof aMonteCarlosimulation.3.4.1AllMeasurementsareofRelativePose Tocomparethecentralizedanddistributedalgorithms,wee xaminethelocalization ofagroupof 5 robots.Allinter-robotrelativemeasurementsareoftherel ative pose.Eachofthe 5 robotstravelsalongadistinctzig-zagpathin3-D,shownin Figure 3-3(a) .Tworobotscanobtainrelativeposemeasurementsattime k ifthe Euclideandistancebetweenthematthattimeislessthan 7m .Duetoassumption 3.2 communicationbetweenrobotsispossiblebetweenthosepai rswithadistanceless than 7m .Furthermore, 25% ofthesepotentialmeasurementsweredroppedtosimulate randomfailure.Aplotofthenumberofneighborsofrobot 1 overtimeisshownin Figure 3-3(b) .Theorientationmeasurementsforeachrelativepose(both inter-robot andinter-time)werecorruptedbyindependentidentically distributed(i.i.d.)unit quaternionsdrawnfromaVonMises-Fisherdistribution[ 55 ]centeredaroundthe zero-rotationquaternionandwithaconcentrationparamet erof 10,000 .Noiseinthe relativetranslationmeasurementswassimulatedbyadding i.i.dzero-meannormal randomvariableswithcovariancematrix I 3 3 10 6 68

PAGE 69

-4 -2 0 2 4 6 8 0 10 20 30 0 5 10 15 20 k =0 k =100 x(m) y(m)z(m)(a) 0 20 40 60 80 100 0 1 2 3 4 Time kj N 1 ( k ) j(b) Figure3-3.Thetrajectoriesandneighborrelationsusedin allsimulations.In(a)the3D trajectoriesforeachthe 5 robotsareshownand(b)showsthenumberof neighborsforrobot 1 asafunctionoftimewhenall 5 robotsarecooperating. Figure 3-4 showsthebiasandstandarddeviationinpositionerrorofas ingle robotactingina 5 robotteam,estimatedusinga100-iterationMonteCarlosim ulation. Thegroupofrobotsperformedlocalizingusingeitherthece ntralizedordistributed collaborativelocalizationalgorithmswithallinter-rob otmeasurementsbeingofthe relativepose.Theplotsindicatethattheimprovementinlo calizationaccuracywith thedistributedalgorithmisquiteclosetothatwiththecen tralizedalgorithm.Thisis promisingsincethedistributedalgorithmisapplicableto largeteamsofrobotsinhighly dynamicscenariosthatcanleadtoarbitrarytimevariation inneighborrelationships.The centralizedalgorithmisnotapplicableinarealisticsett ing,butitprovidesameasureof thebestperformancepossibleusingthisalgorithm.Fromth ispointforward,wewillonly studythedistributedalgorithm. Wenextconsideragroupofrobotsutilizingthedistributed collaborativelocalization algorithm,thistimevaryingthenumberofrobotsinthegrou p.Ineachofthesample runsoftheMonteCarlosimulation,therobotstraveledthes amepaths-thoseshown inFigure 3-3(a) .Measurementnoisewasintroducedinthesamemannerasinth e previoussimulations.Neighborrelationswereagaindeter minedasdescribedearlier, 69

PAGE 70

0 5 10 15 20 25 0 0.2 0.4 0.6 0.8 Distributed Centralized Time kBias(a) 0 5 10 15 20 25 0 1 2 3 4 Distributed Centralized Time k StandardDeviation(b) Figure3-4.Comparisonbetweenthecentralizedanddistrib utedcollaborative localizationalgorithm.AMonte-Carlosimulationswith 50 iterationswasused toestimatethe(a)biasand(b)standarddeviationispositi onestimation error. andkeptthesamefromruntoruntoprecludethatfrombeingan additionalsource ofrandomness.Simulationsforrobotteamsofsize 1,2,3,4 and 5 werecarriedout. Ineachcase 1,000 iterationswereperformed.Whenonlyonerobotispresentint he team,collaborativelocalizationisequivalenttoself-lo calizationwithouttheaidofany inter-robotrelativeposemeasurement.Asthenumberofrobo tsintheteamincreases, thenumberofneighborsforarobotatanygiventimewilltend toincreaseandso greaterimprovementinlocalizationaccuracyisexpected. Thebiasandstandarddeviationinthepositionestimatione rror e i ( k ) forrobot 1 ( i =1 )areshowninFigure 3-5 .Bothbiasandstandarddeviationshowsignicant improvementwithdistributedcollaborativelocalization overself-localization.This isevidentevenforateamofonlytworobots.Asthenumberofro botsintheteam increases,thelocalizationerrorofrobot 1 decreases.Theimprovementinaccuracy however,showsadiminishingreturnwithincreasingteamsi ze.Similarresults wereseenwhenthebiasandstandarddeviationinorientatio nestimationerrorwas considered. 70

PAGE 71

0 20 40 60 80 100 0 0.2 0.4 0.6 0.8 replacements Time kBiasSelfLoc. Collaborative n =1 n =2 n =3 n =4 n =5 (a) 0 20 40 60 80 100 0 1 2 3 4 5 Time k SelfLoc. Collaborative n =1 n =2 n =3 n =4 n =5StandardDeviation(b) Figure3-5.Simulationresultsstudyingthelocalizationac curacyv.s.thenumberof robots.Thesample(a)biasand(b)standarddeviationinthe position estimationerrorforrobot 1 arefoundusingaMonte-Carloexperimentwhen thedistributedalgorithmisusedforcommunicatingrobotg roupsofsize 1 (selflocalization), 2,3,4 and 5 3.4.2HeterogeneousMeasurements Wenowperformsimulationsforthecasewheninter-robotrel ativemeasurements areagainallowedtobeoftherelativepose,orientation,po sition,bearing,ordistance. Toexaminetheeffectthatmeasurementtypehasonlocalizat ionaccuracy,ineach experiment,weletallinter-robotrelativemeasurementsb eofthesametype.Monte Carlosimulationswereconductedinwhichweagainconsider the5robotstravelingin thezig-zagpathsshowninFigure 3-3(a) .Errorsinthepose,orientationandposition measurementsareintroducedasdescribedinSection 3.4.1 .Noiseinthebearing measurementsisinducedbyrotatingthetruebearingthroug htheapplicationofaunit quaterniongeneratedfromani.i.d.VonMises-Fisherdistr ibution.Thenoiseinthe distancemeasurementsisnormallydistributed.Thebiasan dstandarddeviationof positionerrorforrobot 1 areshowninFigure 3-6 (a,b),whilethoseoftheorientationerror areshowninFigure 3-6 (c,d). Weseefromtheplotsthatimprovementoverself-localizati onoccursforall measurementtypes,withtheexceptionofdistancemeasurem ents.Whiledistance 71

PAGE 72

measurementsimprovethestandarddeviationofthepositio nestimates,theyhavelittle ornoeffectonthebias.Thatdistancehaslittleeffectonth eaccuracyisconsistent withtheconclusionsinthestudy[ 17 ]for2-Dlocalization.Thefactthatbearing measurementsleadtohigherimprovementoverdistanceorpo sitionmeasurements wasalsoobservedin[ 17 ]forthe2-Dcase.Whiletheconclusionsin[ 17 ]werebasedon singlesimulations,oursarebasedonMonteCarlosimulatio ns. Trade-offsbetweencostofsensorsandtheresultingbenet inlocalizationcan beanalyzedfromtheseempiricallyobservedtrends.Though fullposeprovidesthe mostbenettolocalizationaccuracy,itisclearthatanyof theconsideredinter-robot measurementtypescanbeusedtoimprovelocalizationaccur acyoverdead-reckoning. Inparticular,afterrelativepose,relativepositionseem stobethemostvaluabletypes ofinter-robotmeasurements,leadingtosignicantreduct ioninbothbiasandvariance overdeadreckoning.Thismeansthatthecostofhavingsenso rscapableofmeasuring relativeposition(stereovision,laserrangender,ormon ocularcamerabasedbearing sensoralongwithaRF-baseddistancemeasurement)mayvery wellbejustiedby thelocalizationaccuracytheyleadto.Ontheotherhand,it isalsoapparentthatthe improvementduetointer-robotbearingmeasurementsisqui tecomparabletothatdue tointer-robotrelativepositionmeasurements.Yet,onlya singlecameraisnecessaryto measurethebearing,whereas(ingeneral)stereovisionisn ecessarytomeasurethe fullrelativeposition,whichisstillquitepronetolargee rrorsunlesslargebaselinestereo isused.Thus,givencost,payload,andreliabilityconstra ints,monocularcamerasmight beabetterchoicethanstereovision.Theseconclusionscom ewiththecaveatthatthey havebeendrawnfromonesetofMonteCarlosimulations;more extensivestudiesare neededtoestablishhowgeneralthesetrendsare. 3.5ExperimentalResults Inthissectionwepresentresultsforexperimentsconducte dwithtwoPioneer P3-DXrobots;theyareshowninFigure 3-7 .Eachrobotwasequippedwithacalibrated 72

PAGE 73

0 100 200 300 400 500 0 5 10 15 20 Time k SelfLoc.Bias( m )Pose Orientation PositionBearing Distance (a) 0 100 200 300 400 500 0 10 20 30 40 50 60 Time k SelfLoc. Pose Orientation PositionBearing DistanceStandardDeviation( m )(b) 0 100 200 300 400 500 0 0.2 0.4 0.6 0.8 Time k SelfLoc. Pose Orientation PositionBearing Distance Bias(radian)(c) 0 100 200 300 400 500 0 0.05 0.1 0.15 0.2 0.25 0.3 Time k SelfLoc. Pose Orientation PositionBearing Distance StandardDeviation(radian)(d) Figure3-6.Simulationresultsstudyingthelocalizationac curacyv.s.measurementtype. Thesample(a)biasand(b)standarddeviationinpositioner rorand(c)bias and(d)standarddeviationinorientationerrorforrobot 1 ,foundusinga Monte-Carloexperiment,areshownforeachmeasurementtyp ewhenthe distributedalgorithmisusedtoperformlocalization.whe nthedistributed Thelabel“SelfLoc.”referstoarobotusingdeadreckoningto localize, withouttheuseofanyinter-robotmeasurements.Thelabels “Pose”, “Orientation”,“Position”,“Bearing”and“Distance”indic atethegroupof robotsusedinter-robotmeasurementsoftherespectivetyp estoperform collaborativelocationusingthedistributedalgorithm. monocularProsillicaEC1020cameraandwheelodometers.Meas urementsfromthese sensorswerefusedtoobtainthenoisyinter-timerelativep osemeasurements.Each robotisadditionallyequippedwithatargetallowingtheon -bardcamerastomeasure 73

PAGE 74

Figure3-7.TwoPioneerP3-DXrobotsequippedwithcamerasand targets.Robot 1 is shownontheleft,whilerobot 2 isontheright. theinter-robotrelativeposebyexploitingtheknowngeome tryofeachtarget.Thetrue poseofeachrobotwasdeterminedusinganoverheadcameraca pableoftrackingeach robot'starget.Thesensorsuitewaspolledevery 0.2 secondswiththenoisyinter-robot relativeposemeasurementsavailableatmost,butnotall,t imes. Allrobotsmovedinstraightlineswiththeirpathsapproxima telyparallel.Six differentposeestimatesoftherobotswereobtainedateach time.Therstwasa dead-reckoningestimate,obtainedfromtheinter-timerel ativeposemeasurements alone.Theremaining5estimateswereobtainedbyusingthed istributedcollaborative localizationalgorithmwiththeinter-robotnoisyrelativ emeasurementsbeingoffull pose,orientationonly,positiononly,bearingonly,ordis tanceonly,respectively.These measurementswereobtainedbyprojectingtherelativepose measurementsonto theappropriatemeasurementtypes,thendiscardingtherem aininginformation. Theresultingglobalpositionestimates,alongwiththetru epositions,forrobot 1 are reportedinFigure 3-8 .SimulationspresentedinSection 3.4 indicatethatweshould seeasignicantimprovementinlocalizationaccuracyeven inthissmallteam,and theexperimentalresultsareconsistentwiththatconclusi on.Asinthesimulations, distinctimprovementinlocalizationaccuracyisseenwhen collaborativelocalizationis performed,irrespectiveofthetypeofinter-robotmeasure ment,thoughtheimprovement varieddependingonthemeasurementtype. 74

PAGE 75

-1 0 1 2 3 4 5 0 2 4 6 8 10 TrueLoc. SelfLoc. Pose Orientation PositionBearing Distance x(m)y(m)Figure3-8.Experimental:Aplotofthelocationofrobot 1 intheoverheadcameraframe ofreferencewhenbothrobotsmoveinastraightline.Thetru epath(found usingtheoverheadcamera),estimatedpathusingselflocal ization,and estimatedpathusingthedistributedcollaborativelocali zationalgorithmare allreported.Thevariouscurvescorrespondtothetypeofin ter-robotrelative measurementused. Toshowthattheimprovementsseeninthepreviousexperimen tsarenotjusta randomoccurrence,theexperimentwasrepeated 24 timestoproduceanempirical estimateofthebiasandstandarddeviationinpositionerro r(bytakingappropriate averages)foreachtypeofinter-robotmeasurement.Theres ultsarereportedin Figure 3-9 .Theexperimentalresultsagainshowthatallmeasurementt ypeslead toanincreaseinlocalizationaccuracy,withrelativepose measurementsleading tothemaximumimprovement,asexpected.Thisisalsothesam etrendthatwas observedinthesimulations.Incontrasttothesimulations ,hereweseethatdistance measurementsdoleadtoanon-negligibleimprovementinthe biasoflocalization accuracy.Asinthesimulations,weseethatbothbearingandp ositionmeasurements leadtosimilarimprovementinthebias.However,incontras ttothesimulations,inthe experimentsorientationmeasurementsseemtoimprovetheb iasmorethanbearing orpositionmeasurements.Thetrendofthestandarddeviati onimprovementwith 75

PAGE 76

orientation,positionandorientationmeasurementsaresi milarinbothsimulationand experiments.Themostsignicantdifferencebetweenthetr endsobservedinsimulations andexperimentsareinthestandarddeviationimprovementb etweenpose,orientation, andpositionmeasurements:theyaremuchcloserintheexper imentsthaninthe simulations.Infact,thestandarddeviationwithposition ororientationmeasurements seemtobealittlesmallerthanthatwithposemeasurements. Webelievethisisan artifactofthesmallnumberofexperimentalsamplesaverag edtoestimatethebiasand standarddeviationempirically,whichlimitstheaccuracy oftheestimates. Inshort,theexperimentsverifythattheproposedalgorith mfordistributed collaborativelocalizationleadstostatisticallysigni cantimprovementinlocalization accuracyevenwithasmallnumberofrobots.Severaltrendsse enintheexperiments abouttherelativemeritsofthedifferenttypesofmeasurem entsareconsistentwith thoseinthesimulations.However,thereareafewnoticeabl edifferencesaswell. Thoughtheexactcauseofthesedifferencesisnotclear,one shouldnotethatthe simulationsandexperimentsdifferinanumberofways.Theb iggestdifferenceisthat thebiasandstandarddeviationsestimatedfromtheexperim entsarelikelytohave highererrorthaninthesimulationsduetothesmallnumbero fexperimentalsamples, whichinturnisduetothedifcultyassociatedwithconduct ingrepeatedexperiments. Theothersignicantdifferencecomesfromthemeasurement noisedistributions.In thesimulations,measurementnoisewasdrawnfromdistribu tionsthatweresomewhat arbitrarilychosen,whilethenoisedistributionsintheex perimentsareunknown.The thirdpotentialsourceofdifferenceisthepathsoftherobo ts.In[ 56 ],whichexamined thegrowthofdead-reckoningerror,weshowedthatthepatha robottraversesplays acrucialroleinthebiasandstandarddeviationofthelocal izationerror.Inparticular, thebiasgrowswithoutboundiftherobotmovesinastraightl ine,butstaysuniformly boundedbyaconstantiftherobotstaysinsideaboundedregi on.Similareffectsare likelyincollaborativelocalization.Therefore,thediff erencebetweenthepathsusedin 76

PAGE 77

0 100 200 300 400 0 0.5 1 1.5 2 2.5 3 3.5 Time k Bias( m ) SelfLoc.PoseOrientationPositionBearingDistance (a) 0 100 200 300 400 0 0.1 0.2 0.3 0.4 0.5 0.6 Time k SelfLoc. PoseOrientationPositionBearingDistanceStandardDeviation( m )(b) Figure3-9.Experimentalresultsstudyingthelocalization accuracyv.s.measurement type.Thesample(a)biasand(b)standarddeviationinposit ionerrorfor robot 1 areshownforeachmeasurementtypewhenthedistributed algorithmisusedtoperformlocalizationonapairofrobots movinginan approximatelystraightline.Thebiasandstandarddeviati onwereempirically estimatedbyaveragingover24repeatedtrials. thesimulationandexperimentsmightbeanothersourceofth edifferenceinlocalization accuracyobserved. 3.6Summary Inthischapterweintroducedanoveldistributedalgorithm forestimatingthe3-D poseofmultiplerobots.Thealgorithmutilizingnoisyinte r-robotmeasurementsof varioustypes(relativepose,orientation,position,bear ing,ordistance)betweenpairs ofrobots,whenavailable,alongwiththenoisyinter-timer elativeposemeasurements usuallyutilizedfordeadreckoning.Thedistributedalgor ithmisinspiredbyacentralized algorithmforsolvingaleast-squarestypeprobleminwhich thenaturalmanifold structureofthespaceofrotationsisutilized.Theleast-s quareslikecostfunctionis chosentomeasurehowwelltheestimatesexplaintherelativ emeasurements.A gradient-descentinaproductRiemannianmanifoldisusedt osolvetheoptimization problemensuringtheestimatesremainonthemanifoldwitho uttheneedforany projection.Theproposedalgorithmdoesnotrelyonanypart icularparameterization oftheunderlyingmanifold;allowinganyparameterization tobeusedinnumerical 77

PAGE 78

computationswithoutaffectingtheresultingestimates.T healgorithmisprovablycorrect inthesensethatthesolutionconvergestoacriticalpointo fthecostfunctionasthe numberofiterations(gradientdescentsteps)increases. 78

PAGE 79

CHAPTER4 EXISTINGMETHODSOFCOLLABORATIVELOCALIZATION InthischapterwecomparetheRPGOalgorithmpresentedinCha pter 3 with existingmethodsusedforcollaborativelocalization.Two state-of-the-artalgorithmsin particularareconsidered.Therstisreferredtohereasth eEuclideanPoseGraph Optimization(EPGO)algorithm.Thisisbasedonstandardpose graphoptimization methods,see[ 57 ]andreferencestherein. Thesecondalgorithmconsideredutilizesanindirectexten dedKalmanlter(IEKF) toperformcollaborativelocalization.Thoughthespecic formofthisalgorithmisoriginal work,itwasdevelopedinasimilarmannerastheIEKFobserverf orcollaborative localizationin[ 58 ].WewillrefertothisastheIEKFalgorithm. Ineachcase,aswiththeRPGOalgorithm,theinputtoeachalgo rithmisafully labeledgraph,aninitialguessforthenodevariables,anda setofnoisyrelative measurements. TheremainderofthisChapterisorganizedasfollows.Insec tion 4.1 wereview theJPLstandforunitquaternionsasaparameterizationof SO (3) .InSection 4.2 the EPGOalgorithmispresented.TheninSection 4.3 theIEKFalgorithmispresented. Simulationscomparingthesetwoalgorithmswith RPGO arepresentedinSection 4.4 4.1UnitQuaternions Inthissection,wewillreviewtheunitquaternionasitrela tetothespace SO (3) Ratherthenusingthestandarddenitionofquaternionspro posedbyHamilton,amore convenientdenitionisoftenusedintheroboticscommunit y.Specically,thestandard conventionproposedbyJPL[ 59 ]. Underthisconvention,theunitquaternionisdenedas q = q 1 i + q 2 j + q 3 k + q 4 79

PAGE 80

where i j ,and k arehyperimaginarynumberssuchthat i 2 = j 2 = k 2 = 1, ijk =1. Wewilloftenrepresent q byacolumnvectorsuchthat q =[ q 1 q 2 q 3 q 4 ] T Multiplicationonthesetofunitquaternions,denotedbyth esymbol n ,isdenedby p n q = 264 p 4 q + q 4 p + b q c p p 4 q 4 p T q 375 where q =[ q 1 q 2 q 3 ] T and b q c isthecrossproductmatrixgivenby b q c = 266664 0 q 3 q 2 q 3 0 q 1 q 2 q 1 0 377775 Unitquaternionsareofinteresttousbecauseeachunitquat ernionmapsto auniqueelementof SO (3) inasmoothfashion.Givensuchaquaternion q ,the corresponding 3 3 rotationmatrixisgivenby C ( q ):=(2 q 2 4 1) I 3 3 2 q 4 b q c +2 q q T Inthefollowingsections,boththeIEKFandEPGOalgorithmswill utilizeunit quaternionsastheirparameterizationof SO (3) 4.2TheEuclideanPoseGraphOptimizationAlgorithm TheRPGOalgorithm,rstpresentedinSection 3.2 ,isaspecialcaseofalarger classofalgorithmscalledposegraphoptimizationalgorit hms.Incontrasttothe RPGOalgorithm,whichutilizesacostfunctionthatisnotdep endentonanyparticular parameterizationof SO (3) ,standardposegraphoptimizationtechniquesrequiresuch asuitableparameterizationtobeused.Inthissectionweco nsideronesuchpopular 80

PAGE 81

parameterizationgivenbythesetofunitquaternions[ 59 ].Werefertothisalgorithmas theEuclideanPoseGraphOptimization(EPGO)algorithm. Weagainutilizethefullylabeled,timevaryinggraphprese ntedinSection 3.1 Foreachnode u 2 V ( k ) ,let q u denotetheunitquaternioncorrespondingtothe nodevariable R u .Similarly,givenaorientationmeasurement ^ R uv ,let ^ q uv denotethe correspondingunitquaternion. Estimatesforthenodevariablesattime k aredeterminedbyminimizingacost function f ( f q u t u g u 2 V ( k ) ):= X ( u v ) / e 2 E ( k ) g e ( q u t u q v t v ) T P e g e ( q u t u q v t v ) (4–1) wherethepositivedenitematrix P e isascalingmatrixand g e ( q u t u q v t v ) isasuitable vectoredgeerrordenedforeachmeasurementtype.Thoughs tandardposegraph optimizationisabletohandleallmeasurementtypesconsid eredthusfar,foreaseof exposition,onlymeasurementsoftherelativepositionand orientationwillbeconsidered inthefollowingdiscussion. Giventheunitquaternionparameterization,nocanonicalv ectoredgeerrorexists. Instead,manychoicesarepossible.Onesuitablechoice,wh ichisthevectoredgeerror usedinthesubsequentcomparisons,isgivenasfollows: g e ( q u t u q v t v )= 8>><>>: q 1 u n q v n ^ q 1 uv 1 if ` ( k )( e )= R C ( q u ) T ( t v t u ) ^ t uv if ` ( k )( e )= t (4–2) where e ( u v ) n denotesquaternionmultiplicationasdenedinSection 4.1 ,and C denotethemapthattakesaunitquaterniontoitscorrespond ing 3 3 rotationmatrix representation. Manyvector-spaceoptimizationalgorithmscanbeusedtose archfortheminimum ofsuchacostfunction.OnecommonchoiceistouseLevenberg -Marquadt,aswas donein[ 57 ].ToimplementLevenberg-Marquadt,aminimalparameteriz ationisutilized. 81

PAGE 82

Givenaunitquaternion q u ,wedenotethecorrespondingminimalparameterizationas q u suchthat q u T q 4u T = q Tu .Inaddition,anewoperator : Dom ( q u ) Dom ( q u ) Dom ( q u ) isdenedby q u q v = q v n q u (4–3) Tosimplifythediscussion,theparameterizednodevariabl es,minimallyparameterized nodevariables,andvectoredgeerror,andscalingmatrices areallstackedasfollows: X = t T1 q T1 ,..., t Tn q Tn T X =[ t T1 0 q 1 0 T ,..., t Tn 0 q 1 0 T ] T g =[ g T 1 ,..., g T m ] T P = diag ( P 1 ,... P m ) ,whereforall i 2 V ( k ) t i 0 2 R 3 q i 0 2 Dom ( q i ) .Finally,we extendthe operatortoactonthenewstackedstatevectorsas X X =[( X 1 X 1 ) T ,...,( X 2 n X 2 n ) T ] T wherethe isdenedforunitquaternionsin( 4–3 )andreducestotheadditionoperator forpositionvectors.TheJacobianofthestackedvectorerr orfunction g isgivenby J = @ g i (X X) @ X j X=0 ij Forthemeasurementtypesconsidered,theJacobianisexpli citlygivenasfollows. Consideranedge e ( u v ) correspondingtoanorientationmeasurement.Tosimplify thenotation,let q = q 1 u and p = q v n ^ q 1 uv .Then @ g e ( X X ) @ q h X=0 = 264 p 4 b q c p 4 q 4 id q 4 b p c + b p cb q c p 4 q T + q 4 p T p T b q c 375 I u v ( h ) @ g e ( X X ) @ t h X=0 =0 82

PAGE 83

Where I u v ( h )=1 if h = u 1 if h = v 0 otherwise,and b v c isthecrossproductmatrix givenby b v c = 266664 0 v 3 v 2 v 3 0 v 1 v 2 v 1 0 377775 for v =[ v 1 v 2 v 3 ] T .Similarly,when e correspondstoapositionmeasurementwend @ g e ( X X ) @ q h X=0 = 2 b ( t v t u ) c I u ( h ) @ g e ( X X ) @ t h X=0 = I u v ( h ) C ( q u ) T Where I u istheindicatorfunction. Levenberg-Marquadtisthencarriedoutintheusualfashion ,setting H = J T PJ b = J T Pg ( X ) .Fixingatime k ,ateachiterationoftheLevenberg-Marquadtalgorithm, theupdatestatevector X isgivenbysolvingthelinearequation ( H + id ) X = b fora suitablylarge .Thecurrentestimateof X isthenupdatedbysetting X X X .This processisrepeateduntil k X k isappropriatelysmall. Remark4.1. Theorientations( R ( ) )thatappearinourproblemformationsandthrough outthedevelopmentoftheRPGOalgorithminChapter 3 areabstractrotationoperators, orelementsof SO (3) .Noparticularparameterization(quaternions,rotationm atrices, etc.)isassumedduringthedevelopment.Incontrast,theco stfunctionfortheEPGO algorithm, ( 4–2 ) utilizesaparticularparameterizationintheformofunitq uaternions. Adifferentparameterizationwouldnecessarilyleadtoadi fferentcostfunction,and perhapsadifferentestimatedeliveredbythecorrespondin galgorithm.Thus,the estimatesfoundusingtheEPGOalgorithmareafunctionofboth themeasurementsand thechosenparameterization,ratherthenafunctionofthem easurementsalone. 83

PAGE 84

4.3TheImplicitExtendedKalmanFilterAlgorithm TheIEKFalgorithmpresentedblowissimilartothatfoundin[ 58 ].Thetwoprimary differencesareasfollows: ( i ) Inter-timerelativemeasurementsareoftherelativepose, ratherthenmeasurementsofthelinearandangularvelociti es.Thisismotivatedbythe ideathatmeasurementsarecapturedbyacamera,ratherthen aninertialmeasurement unit. ( ii ) TheIEKFalgorithmutilizesmeasurementsoftherelativeposi tionbetween robots,whereasthealgorithmpresentedin[ 58 ]usesmeasurementsofthebearingwith respecttoatimevaryingsetoffeaturepoints.Underthesec hanges,theIEKFalgorithm isabletofunctionunderthesamecircumstancesastheRPGOal gorithm. TheIEKFalgorithmutilizesanindirectEKF.Alsosometimesrefer redtoasan error-stateEKF.AbriefreviewoftheindirectEKFisprovidedbe low. 4.3.1TheImplicit(Error-State)ExtendedKalmanFilter Considerastatevector X k withthe(possibly)non-linearstatetransitionand observationmodels X k +1 = f ( X k )+ g ( k ) (4–4) z k +1 = h ( X k +1 )+ k +1 (4–5) where k and k areassumedtobewhite,zeromean,andmutuallyindependent ,with covariance Q k and R k respectively. Let ^ X k j k 0 denoteanestimateofthestateattime k givenallmeasurementsupto time k 0 obtaineddirectlyfromthosemeasurements(suchasintegra tionofIMUdata). Theerror-stateisdenedas ~ X k = X k ^ X k (4–6) Inthefollowing, ^ X k +1 j k ^~ X k +1 j k denotesapredictionofthestateattime k +1 based onmeasurementsuptotime k .Similarly, ^ X k +1 j k +1 ^~ X k +1 j k +1 denotestheestimatedstate afterallmeasurementsattime k +1 havebeenconsidered. 84

PAGE 85

ThepropagationstepoftheindirectEKFisasfollow. ^ X k +1 j k = f ( ^ X k j k ) (4–7) ^~ X k +1 j k = f ( ^~ X k j k + ^ X k j k ) f ( ^ X k j k ) (4–8) P k +1 j k = FP k j k F T + GQ k G T (4–9) where P k j k isthecovarianceestimateof X k andthusof ~ X k givenallmeasurementsupto time k ,and F G aretheJacobianmatrices F = @ f ( X ) @ X X= ^ X k j k G = @ g ( ) @ =0 (4–10) Byconstruction,wewillshowthatforall k ^~ X k j k =0 .Combiningthisfactwith( 4–9 )gives ^~ X k +1 j k =0 Givenameasurementattime k z k ,theerror-statemeasurementisdenedas ~ z k = z k ^ z k where ^ z k = h ( ^ X k ) .Thelinearizederror-statemeasurementisthengivenby ~ z k +1 = H k +1 ~ X k +1 + k +1 (4–11) where H k +1 = @ h ( X ) @ X X= ^ X k j k Let S k +1 = H k +1 P k +1 j k H T k +1 + R k +1 .TheKalmangainis K k +1 = P k +1 j k H T k +1 S 1 k +1 and theupdatestatein X = K ~ z .TheindirectEKFstateisthenupdatedtoincludethe measurement z by ^~ X 0k +1 j k +1 = ^~ X k +1 j k + X = X P k +1 j k +1 =( I K k +1 H k +1 ) P k +1 j k ( I K k +1 H k +1 ) T + K k +1 R k +1 K T k +1 85

PAGE 86

Theprimedenotesthatthisisonlytemporarilytheupdateds tate,as ^~ X 0k +1 j k +1 neednot equalzero.Toavoidtheneedtopropagatebothanestimateof X andtheerror-state, aresetofoftheerror-stateisnecessary.Weresettheerror -statebyincludingthe informationitholdsintheestimate ^ X k +1 j k +1 .Notethat ^~ X 0k +1 j k + ^ X k +1 j k isitsselfan estimateofthestate X .Anyupdatetotheerror-stateestimateshouldnotchangethe totalstateestimate.Thisleadstothefollowingdenition ^ X k +1 j k +1 = ~^ X 0k +1 j k +1 + ^ X k +1 j k Thisdenitionensuresthat ^~ X k +1 j k +1 =0 Twoimportantfactsarisefromtheproposedreset.Thersti sthatthecovariance estimateoftheerror-stateisunchangedbythereset.These condisthattheerror-state estimateneedneverbeexplicitlycomputed.Theupdateandr esetarecombinedtogive ^ X k +1 j k +1 = ^ X k +1 j k + X (4–12) 4.3.2CollaborativeLocalizationUsingtheIndirectEKF AswiththeEPGOalgorithm,andincontrasttotheRPGOalgorithm, theIEKF algorithmrequiresasuitableparameterizationof SO (3) tobeused.Unitquaternionsare againchosenforthisparameterization.Toremainconsiste ntwiththenotationusedin theliterature,arotationwillbeparameterizedbyitsinve rsequaternion.Thatis,givena nodevariable R u where u mapstotime k androbot i ,thecorrespondingquaternion q ki is givenby q ki = C ( R Tu ) (4–13) where C denotesthemapthattakesaunitquaterniontoitscorrespon ding 3 3 rotation matrixrepresentation. Thepreviousdenitionintroducesanewnotation,disjoint fromtherestofthe chapter,butnecessarytoproperlydescribetheIEKFalgorith m.Fortheremainderof 86

PAGE 87

thissection,unlessspecicallynotedotherwise,supersc riptnotationwilldenotetime andsubscriptwilldenoterobotindex. Thestatevectorforrobot i attime k isgivenby X ki =[( q ki ) T ( t ki ) T ] T (4–14) where t ki isthepositionofrobot i attime k and q ki istheorientationofrobot i andtime k asdenedin( 4–13 ).Thefullstatevector X k isgivenbystackingthestatevectorfor eachrobotand ^ q ki ^ t ki ^ X ki ^ X k denoteestimatesofthecorrespondingtruestates. Theerrorstatevectorforrobot i attime k correspondingto( 4–14 )isgivenby ~ X ki =[( ki ) T ( ~ t ki ) T ] T (4–15) where ki istheangle-errorvectordenedbytheerrorquaternion ~ q ki = q ki n (^ q ki ) 1 [ 1 2 ( ki ) T 1] T .Thisapproximationismadebyassumingthat d ( id C (~ q ki )) issufciently small.Thestatepropagationmodelischosentocorrespondw iththekinematicmodel wheninter-timerelativepositionandorientationmeasure mentsareavailable, q k +1 i =( q k k +1 i ) 1 n q ki (4–16) t k +1 i = t ki + C ( q ki ) T t k k +1 i (4–17) where q k k +1 i t k k +1 i denotethechangeinorientationandpositionrespectively forrobot i betweentime k andtime k +1 .Themeasuredchangeinorientationandpositionfor robot i ismodeledas ^ q k k +1 i =~ q k k +1 i n ( q k k +1 i ) 1 and ^ t k k +1 i = t k k +1 i + ~ t k k +1 i where ~ q k k +1 i [ 1 2 ( k k +1 i ) T 1] T .Here k k +1 i and ~ t k k +1 i areassumedtobezero-mean whiteGaussiannoiseprocesses.Thelinearizederrorstate equationisthengivenby ~ X k +1 = F k ~ X k + G k [( k k +1 1 ) T ( ~ t k k +1 1 ) T ...( k k +1 n ) T ( ~ t k k +1 n ) T ] T for F k = diag ( F k i ) 87

PAGE 88

G k = diag ( G k i ) and F k i = 264 I 3 C (^ q ki ) T b ^ t k k +1 i c 0 C (^ q k k +1 i ) T 375 G k i = 264 C (^ q ki ) T 0 0 C (^ q k k +1 i ) T 375 Where b v c isdenedasinSection 4.2 Ateachtimestep,thenoisymeasurementsoftherelativechan geinpositionand orientationareusedtoupdatethestateestimateandcovari anceas ^ X k +1 j k i = 264 (^ q k k +1 i ) 1 n ^ q ki ^ t ki + C (^ q ki ) T ^ t k k +1 i 375 and P k +1 j k = F k P k ( F k ) T + G k Q k ( G k ) T where Q k =E[[( k k +1 1 ) T ( ~ t k k +1 1 ) T ...( k k +1 n ) T ( ~ t k k +1 n ) T ] T [( k k +1 1 ) T ( ~ t k k +1 1 ) T ...( k k +1 n ) T ( ~ t k k +1 n ) T Herethesuperscript k +1 j k isusedtoindicatethisisatemporaryestimateofthestate attime k +1 utilizingonlytheinter-robotrelativemeasurementsavai lableuptotime k .Ifnointer-robotmeasurementsareavailableattime k +1 ,then ^ X k +1 i = ^ X k +1 j k i P k = P k +1 j k .Wheninter-robotmeasurementsareavailable,theyareutil izedasfollows. Givenanedge e ij 2 E ( k ) correspondingtoaninter-robotmeasurementattime k from robot i to j ,themeasurementismodeledby z ke ij = C ( q ki )( t kj t ki )+ e ij Thenoise e ij isassumedtobeazero-mean,normallydistributedwith E[ e ij ( e ij ) T ]=: R k .Thelinearizederror-measurementmodelisthengiven ~ z ke ij = z ke ij ^ z ke ij H k e ij ~ X k + e ij 88

PAGE 89

where ^ z ke ij := C (^ q ki ) ^ t kj ^ t ki H k e ij = e Ti H k i + e Tj H k j Inthepreviousequation referstotheKroneckerproduct, e i indicatesthe ith standard basisvector,and H k i H k j are H k i = C (^ q ki ) b C (^ q ki ) ^ t kj ^ t ki c H k j = C (^ q ki )0 Finally,giventhesetofmeasurements z ke 1 z ke m acquiredattime k ,thetotal error-measurement ~ z k anderror-measurementmatrix H k aregivenbystackingallthe error-measurements ~ z ke ij andmatrices H k e ij respectively. TheIEKFisthenimplementedasfollows.Let S k +1 = H k +1 P k +1 j k ( H k +1 ) T + R k +1 TheKalmangainis K k +1 = P k +1 j k ( H k +1 ) T ( S k +1 ) 1 andtheupdatestateis X k = K k +1 ( z k +1 H k +1 ^ X k +1 j k ) .TheIEKFstateisthenupdatedtoincludethemeasurement z by ^ X k +1 = ^ X k +1 j k X k +1 P k +1 j k +1 =( I K k +1 H k +1 ) P k +1 j k ( I K k +1 H k +1 ) T + K k +1 R k +1 ( K k +1 ) T where isdenedcomponentwiseasfollows. ^ X k +1 j k i X k +1 i =: 264 ^ q k +1 j k i ^ t k +1 j k k 375 264 k +1 i t k +1 i 375 = 264 q k +1 i n ^ q k +1 j k i ^ t k +1 j k i + t k +1 i 375 where q k +1 i = 1 2 ( k +1 i ) T 1 1 4 k +1 i 1 = 2 T 89

PAGE 90

4.4SimulationResults Inthissectionwepresentsimulationscomparingthecollab orativelocalization algorithmrstpresentedinChapter 3 againstthetwostateoftheartcompeting collaborativelocalizationalgorithmspresentedinSectio n 4.2 andSection 4.3.1 Thefollowingdenitionswillbeofusewhenevaluatingthee ffectivenessofthe variousalgorithms.Givenasetofrobots,thepositionesti mationerrorofrobot i is denedas e i ( k ):= ^ t i ( k ) t i ( k ) ,where t i ( k ) isitsabsolutepositionat k and ^ t i ( k ) is theestimate.Thebiasinthepositionestimationerrorofro bot i isdenedas k E[ e i ( k )] k where kk isthe2-normand E denotesexpectation.Thestandarddeviationisdened as p Tr [ Cov ( e i ( k ), e i ( k )) ] ,where Cov ( ) standsforcovariance.Ineachscenario describedbelow,thebiasandvarianceinpositionestimati onerrorisestimatedthrough theuseofaMonteCarlosimulation. WhencomparingwiththeEPGOalgorithm,allinter-robotrelati vemeasurements areoftherelativepose.WhencomparingwiththeIEKFalgorithm ,allinter-robotrelative measurementsareoftherelativeposition. WerstconsiderthestandardposegraphalgorithmEPGO,inwhi choptimal robotposesarecomputedbyminimizingthecostfunction( 4–1 ).Recallthatrotations areparameterizedbythecomplexpartofthecorrespondingu nit-quaternion,andthe optimizationproblemissetupasin[ 57 ].Searchingfortheoptimaisperformedby Levenberg-Marquardtalgorithm.Tomaintaincomparabilit y,weprovidethesamelocal measurementgraphtoboththeRPGOalgorithmaswellastotheEPG Oalgorithm. Agroupof 5 arerobotsaresimulatedtomovealongthe3-Dpathdescribed above. Errorintheposemeasurementswereinducedasinsimulations inSection 3.4.2 Simulationswereperformedvaryingtheconcentrationparam eter K intheVon Mises-Fisherdistributionfromwhichthenoisyrotations( quaternions)usedtocorrupt theinter-robotorientationmeasurementsaredrawn.These simulationsshowthat, when K isverylarge,thatis,thevarianceisverylow,EPGOdoesveryw ell,even 90

PAGE 91

0 50 100 0 0.2 0.4 0.6 0.8 1 1.2 0 50 100 0 2 4 6 8 SelfLoc. RPGO EPGO Bias( m ) StandardDeviation( m ) Time k (a) 0 50 100 0 0.2 0.4 0.6 0.8 1 1.2 0 50 100 0 2 4 6 8 SelfLoc. RPGO EPGO Bias( m ) StandardDeviation( m ) Time k (b) Figure4-1.SimulationresultscomparingtheRPGOandEPGOalgor ithms.Theposition estimationerrorofrobot1when(a) K =10000 and(b) K =100 (inagroup of 5 robotsutilizingnoisyinter-robotrelativeposemeasurem ents),computed withbothalgorithms,withthesameinputdataareshown.The label“Self Loc.”referstoarobotlocalizingbydeadreckoningalone. outperformingtheRPGOalgorithm.However,when K issmall,thatis,thenoise varianceislarge,RPGOoutperformsEPGO.Figure 4-1 showsthebiasandstandard deviationforboth k =1,0000 and K =100 .Forthecaseof K =100 ,itisclearthatthe proposedRPGOalgorithmoutperformstheEPGOalgorithm.Adeta iledcomparisonisa topicoffuturework. 91

PAGE 92

WenextconsiderIEKFalgorithm,presentinsection 4.3 .TheIEKFalgorithm utilizesanExtendedKalmanFilter(EFK),developedinasimilar mannerastheEKF observerforcollaborativelocalizationin[ 58 ].Anindirectformlterisused,witherror betweenthetrueposeandtheestimatedpose(beforefusingt hecurrentinter-robot measurements)beingthelterstate.Apairofrobotsissimu latedtravelingalongdistinct sinusoidalpathsin3-Dspace.Measurementsaregenerateda sdescribedearlier. Thetrendsobservedfromextensivesimulationscanbesumma rizedasfollows. Whenthetimeintervalbetweensuccessiveinter-robotmeasu rements,callit T issmall,theIEKFalgorithmperformsaswell,orbetterthan,t heRPGOalgorithm. However,whenthetimebetweenmeasurementsislarge,theRPG Oalgorithmprovides signicantlybetterestimatesoftherobots'posescompare dtotheIEKF.Figure 4-2 providesnumericalresultsforboththecaseofasmall T (0.1seconds)andalarge T (30secondsinthisexample).AsexpectedtheIEKFdoesverywell when T is small,butperformspoorlyforthelarger T .Howsmall T hastobefortheIEKF algorithmtoperformwelldependsonmanyfactors,includin gthemotionoftherobots, noiseinthemeasurements,etc.Fortheparametersusedinth esimulationsmentioned above, T hastobesmallerthan 1 secfortheIEKFalgorithmtoperformaswellasthe RPGOalgorithm. WebelievethereasonforthisbehavioroftheIEKFalgorithmis theerrorintroduced bythelinearizationinvolvedincovariancepropagation.T helinearizedstateequations relyontheassumptionthattheanglebetweenthetrueandest imatedorientationis verysmall.Whenthetimeintervalbetweeninter-robotmeasu rementsissufciently small,thisapproximationholds.Inthatcasetheerrorinth ecovariancematrixdue tolinearizationissmallenoughthatitdoesnotoutweighth eaddedbenetofusing covarianceinformation.However,thesmallangleapproxim ationisviolatedforlargetime intervals,leadingtoquitepoorcovarianceestimates,whi chinturnleadtopoorpose estimates. 92

PAGE 93

0 50 100 0 10 20 30 40 50 60 70 0 50 100 0 2 4 6 8 10 x 10 4 SelfLoc. RPGO IEKF Bias( m ) StandardDeviation( m ) Time k (a) 0 50 100 0 50 100 150 200 250 300 350 0 50 100 0 1 2 3 4 5 6 x 10 5 SelfLoc. RPGO IEKF Bias( m ) StandardDeviation( m ) Time k (b) Figure4-2.SimulationresultscomparingtheRPGOandIEKFalgor ithmswith (a) T =0.1 and(b) T =30 sec.Thepositionestimationerrorofrobot1(in agroupof 2 robotsutilizingnoisyinter-robotrelativeposemeasurem ents), computedwithbothalgorithms.Thelabel“SelfLoc.”referst oarobot localizingbydeadreckoningalone. 4.5Summary Inthischapter,twostateoftheartcollaborativelocaliza tionalgorithmswere comparedwiththeRPGOalgorithmpresentedinChapter 3 .Therstalgorithm,referred toastheEuclideanPoseGraphOptimizationalgorithm,wasac lassicalleastsquares basedposegraphoptimizationalgorithm.Thesecondalgori thmwasbasedona 93

PAGE 94

indirectextendedKalmanlter.Ineachcase,simulationswe representedthatidentied situationsunderwhichtheRPGOprovidedmoreaccurateposee stimates. 94

PAGE 95

CHAPTER5 MAXIMUMLIKELIHOODESTIMATES InChapter 3 wepresentedanalgorithmforcollaborativelocalizationw hen inter-robotrelativemeasurementscanbeoftherelativepo sition,orientation,bearing, ordistance.Inthischapterweextendthisalgorithmbyaddi ngaweightassociated witheachedge.Todevelopasystematicwaytodenesuchweig hts,themaximum likelihood(ML)estimatorofthenodevariablesisconsider ed.Wewillrefertothe algorithmdevelopedinthischapterastheMaximumLikeliho odRiemannianPoseGraph Optimization(ML-RPGO)algorithm. 5.1TheMLEstimates Considerthecollaborativelocalizationproblemasdescri bedinSection 3.1 .The fullylabeledtimevaryingmeasurementgraphisgivenby G ( k )=( V ( k ), E ( k ), ` ( k )) Let n ^ M e o e 2 E ( k ) denotethecorrespondingsetofinter-robotandinter-time relative measurements.Weagainconsiderinter-robotrelativemeas urementsoftherelative pose,orientation,position,bearing,anddistance.Forea seofexposition,throughoutthis chapterwewillfreelyidentifyposemeasurementswiththec orrespondingorientation andpositionpair.Wewillthereforeoftennotexplicitlyco nsidermeasurementsoftype pose. Weassumethatmeasurementsondistinctedgesarestatistic allyindependent. Underthisassumption,thejointpdfofallthemeasurements attime k ,givenbytheset ^ M E ( k ) ,satises: p ^ M = Y e 2 E ( k ) p e ( ^ M e ), (5–1) where p e ( ^ M e ) isthepdfdescribingtheprobabilityofobservingthemeasu rement ^ M e Foreachofthefourtypesofmeasurements,acorrespondingc lassofpdfsmustbe specied.Choosingappropriatepdfsfortheorientationan dbearingmeasurements ischallengingasthesedensitiesarenotdenedoveranyvec torspace,butrather 95

PAGE 96

overthecurvedsurfaces SO (3) and S 2 respectively.Weassumethateachrelative orientationmeasurement ^ R ij comesfromawrappedGaussiandistributionon SO (3) withmeandirection R Ti R j andcovariancematrix 2 I .Thecorrespondingdensityfunction p R : SO (3) R + isgivenby f R ( ^ R ij )= K R 1 X k = 1 exp 1 2 2 e d ( ^ R ij R Ti R j ) 2 k 2 (5–2) forappropriatenormalizingconstant K R ( e ) [ 60 ].Herethedistancefunction d ( ) in SO (3) isgivenbytheRiemanniandistance d ( A B )= r 1 2 Tr log 2 ( A T B ) A B 2 SO (3). (5–3) ThoughtacharacterizationoftheWGdistributiononSO(3)iss tillanopenproblem,It isknownthatthecorrespondingWGdistributionon SO (2) isthesolutiontotheheat equation[ 60 ].TheNormaldistributionsolvestheheatequationonavect orspace,and sowecansaytheWGdistributionis“Gaussian-like”. Forbearingmeasurements,wechoosetheVonMises-Fisher(VM F)distribution[ 47 ]. Thisisawell-knowndensityintheliteratureondirectiona lstatisticsandconsidered areasonableanalogoftheGaussiandensityinthe d -Sphere.Specically,each relativebearingmeasurement ^ ij isassumedtobedistributedaccordingtotheVon Mises-Fisherdistributionwithmeandirection := R Ti (t j t i ) k t j t i k andconcentrationparameter k e .Thedensityfunction p : S 2 R + isgivenby p (^ ij )= K exp k e k t j t i k ( t j t i ) T R j ^ ij (5–4) forappropriatenormalizationconstant K ( k e ) .TheVMFdistributionisa“Gaussian-like” distribution,inthatitistheequilibriumdistributionfo rtheOrnstein-Uhlenbeckprocess ontheunitsphere,justastheNormaldistributionistheequ ilibriumdistributionforvector spaces[ 61 ]. 96

PAGE 97

Eachrelativepositionmeasurement ^ t ij isassumedtobedistributedaccordingto themultivariatenormaldistributionwithmean t := R Ti ( t j t i ) andcovariancematrix e .Thedensityfunction p t : R 3 R + isgivenby p t ( ^ t ij )= K t exp 1 2 ( ^ t ij t ) T 1 e ( ^ t ij t ) (5–5) forappropriatenormalizationconstant K t ( e ) Finally,eachrelativedistancemeasurement ^ ij isassumedNormallydistributed withmean k t j t i k andvariance 2 e .Thedensityfunction p : R R + isgivenby p ( ^ ij )= K exp ( ^ ij k t j t i k ) 2 2 2 e (5–6) forappropriatenormalizingconstant K ( e ) .Weassumethatthestandarddeviation e issmallenoughwithrespecttothemean k t j t i k sothattheprobabilityof ^ ij being negativeisnegligible. Amongthesedistributions,thewrappedGaussianisthemostc umbersomedueto theinniteseriesinitsdenition.Wethereforeapproxima te p R bythefunction p R ( ^ R ij )= K R exp 1 2 2 d 2 ( ^ R ij R Ti R j ) (5–7) Notethat p R isnotaprobabilitydensityfunction.Tojustifytheapprox imationof p R by p R ,the1-normof p R p R wascomputedusingMonte-Carlointegrationwith 100,000 samples.Thevalueof e wasintherange [0 + ,2] .Theresultsarereported inFigure 5-1 .For < 0.7 radians,thenormofthedifferenceisnearenoughtozeroto beindistinguishable.Wethereforeconcludethattheappro ximation p R ofthewrapped Gaussiandistribution p R isreasonablyaccurateforvaluesof < 0.7 WearenowreadytocharacterizetheMLestimateofthenodeva riables(absolute poses). 97

PAGE 98

0 0.5 1 1.5 2 0 0.05 0.1 0.15 0.2 0.25 0.3 e (rad)k p R p R k 1Figure5-1.Themagnitudeofthedifferencebetween p R and p R ,thepdffororientation measurementsanditsapproximationrespectively Proposition5.1. AnapproximationofthemaximumlikelihoodMLestimate ( ^ R ^ t ) V ( k ) ofthenodevariables T u u 2 V ( k ) basedonthemeasurements ^ M E ( k ) isgivenby ( ^ R ^ t ) V ( k ) =argmin f (R,t) g V ( k ) 2 ( SO (3) R ) j V ( k ) j f ( T u u 2 V ( k ) ) (5–8) where f :( SO (3) R ) j V ( k ) j R isacostfunctiongivenby f ( T u u 2 V ( k ) ):= X ( i j )= e 2 E ( k ) g e ( R i t i R j t j ) (5–9) inwhich g e ( R i t i R j t j ) isthecostforedge e denedas g e ( R i t i R j t j )= 8>>>>>>>>>>>><>>>>>>>>>>>>: 1 2 2 e d 2 ( ^ R ij R Ti R j ) if ` ( e )= R 1 2 ^ t ij R Ti ( t j t i ) 1 e ^ t ij R Ti ( t j t i ) if ` ( e )= t k e k t j t i k ( t j t i ) T R i ^ ij if ` ( e )= 1 2 2 e ^ ij k t j t i k 2 if ` ( e )= (5–10) 98

PAGE 99

Proof. Asdescribedabove,thepdfdescribingtheprobabilityofobs ervingameasurement dependsonthevalueofthenodevariables.Toemphasizethis ,werewrite( 5–1 )as p ( ^ M E ( k ) j T u u 2 V ( k ) )= Y e 2 E ( k ) p e ( ^ M e j T u u 2 E ( k ) ) wherethedependencyontheunknownparameters T u u 2 V ( k ) isshownclearly.The likelihoodfunction L ( ) isthedensityviewedasafunctionoftheunknownparameters Thelog-likelihoodfunction log L satises log L ( T u u 2 V ( k ) j ^ M E ( k ) )=log p ( ^ M E ( k ) j T u u 2 V ( k ) ) / log( Y e 2 E ( k ) Ker p e ( ^ M e j T u u 2 V ( k ) )) where Ker p ^ M isthekernelofthecorrespondingpdf.Themax-likelihoode stimateis obtainedbymaximizingtherighthandsideoftherelationab ove.Whenweusethe approximation p R insteadof p R ,itfollowsfromstraightforwardcalculationsthattherig ht handsideisequalto f ( T u u 2 V ( k ) ) ,were f isasdenedin( 5–9 ).Thecorresponding (approximate)maximumlikelihoodestimatefor T u u 2 V ( k ) given ^ M E ( k ) iscomputed byminimizing f .Thisestimateisnotstrictlyequaltothemaximumlikeliho odestimate becauseoftheapproximationof p R by p R .Sincetheapproximationisquiteaccuratefor < 0.7 ,weexpecttheestimateobtainedtobeacloseapproximation oftheMLestimate for < 0.7 2 Thecostfunction 5–9 canbeminimizedasdescribedinChapter 3 5.2Simulationstudies Inthissectionwepresentsimulationsstudyingtheperform anceoftheML-RPGO algorithmintermsoflocalizationaccuracy.Foreaseofexp osition,weconsiderthe robotgroupatasingletimeinstant,say k = k 0 ,wheretheonlyinterrobotrelative measurementsavailablearethosecapturedattime k 0 .Thiswillallowustocomparethe 99

PAGE 100

Orient. ( e ) Pos. ( e ) Bearing( k e ) Dist. ( e ) InitialEst. 0.26 rad 4 I m Inter-Cam. 0.087 rad 0.25 I m 20 0.5m Table5-1.Theparametersofthemeasurementpdfsusedinall simulations (see( 5–2 )-( 5–6 )). ML-RPGOalgorithmwithonepresentedin[ 52 ]inwhichcameranetworks,insteadof robots,areconsidered. Weexaminethefollowingquestions.One,howdoesestimatio naccuracyofthe ML-RPGOalgorithmchangeastheconnectivityofthemeasurem entgraphincreases duetotheincreaseinthenumberofrelativemeasurementsfo rthesamenumberof robots/cameras,andhowdoesaccuracydependonthetypeoft hosemeasurements? Two,howdoesML-RPGOperformcomparedtothealternativemet hodproposed in[ 52 ]? Thefollowingdenitionsarerequired.Theerrorinanestim ate ^ R i oftheorientation foracamera i is e R ( i ):= d ( R i ^ R i ) ,where d ( ) isdenedin( 5–3 ).Theerrorinan estimate ^ t i ofthepositionofcamera i is e p ( i ):= k t i ^ t i k 2 .Thetotalr.m.s.errorin theorientationandpositionestimateisdenedas p E[ P ni =1 e 2 R ( i )] and q E[ P ni =1 e 2 p ( i )] respectively,where E[ ] denotesexpectation.Theexpectedvalue E[ e [ ] ( i )] isalso referredtoasthebiasinthaterror.Allexpectationsarecom putedfromappropriate averagingfromrandomsamplesobtainedthroughsimulation s. 5.2.1Performanceinasingleexperiment Weconsideranetworkof 5 cameras.Onepossiblegraphonsuchanetworkis presentedinFigure 5-2 (a).Boththeinitialguessfortheposeofeachcamera,and thenoisyinter-camerameasurementsaredrawnfromthedist ributionsdescribed inSection 5.1 .TheparametersforeachdistributionarereportedinTable 5-1 .The variancesoftheinitialguessofposesarechosentobemuchh igherthenthoseforthe inter-camerameasurementstosimulatearealisticsituati onwhentheinitialguessis poor. 100

PAGE 101

15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (a) GroundTruth 15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (b) InitialEstimate 15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (c) Orientation&Position Meas. 15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (d) OrientationMeas. 15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (e) PositionMeas. 15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (f) BearingMeas. 15 10 5 0 5 5 0 5 10 0 5 10 15 x y z (g) DistanceMeas. Figure5-2.Asinglerealizationforagroupofrobotsusingv ariousmeasurementtypes andtheML-RPGOalgorithm.Eachofthe(a)truevalues(b)thein itialguess fortheposesoftherobots,and(c-g)theestimatedposesaft erthe ML-RPGOalgorithmhasbeenusedwithinter-robotrelativeme asurements. Eachoftheplotsin(c)-(g)correspondtoadistincttypeofre lative measurement. TheML-RPGOalgorithmisappliedtoonerealizationofthenoi symeasurementsfor eachofthemeasurementtypes,aswellasforthecasewhenbot hrelativeorientation andpositionmeasurementsareavailable.Theresultingest imatedpositionsand orientationsareshowninFigures 5-2 (c-g).Weseethatrelativeorientationmeasurements allowforanaccurateestimationoftheorientationofeachc amerawithouthavingany effectontheestimatedposition.Incontrast,relativepos itionmeasurementsimprove boththeorientationandpositionestimatesforeachcamera .Thisisexpectedsince 101

PAGE 102

absoluteorientationsofthecamerasaffecttherelativepo sitionswhileabsolutepositions donotaffectrelativeorientations.Whennoisymeasurement sofbothorientationand positionareavailable,theposeofeverycameracanbeestim atedwithhighaccuracy. Incontrasttothepositionandorientationmeasurements,b oththebearinganddistance measurementsleadtopoorestimates,atleastforthissetof measurements. 5.2.2Effectofmeasurementnoise,andcomparisonwithstate -of-the-artcamera networklocalizationalgorithm Wenowexaminetheeffectonlocalizationaccuracywhensome ofthemeasurements arenoisierthanothers.WealsocompareML-RPGO'sperforman cewiththealgorithm in[ 52 ]forvaryingmeasurementnoiselevels. Anetworkof 3 camerasisconsidered,inwhicheverycamerahasameasureme nt foreveryothercamerainthenetwork.Thatis,thecorrespon dinggraphisfully connected.Eachinter-camerameasurementisoftherelative orientationandbearing. Thesemeasurementtypesarechosentoenablecomparisonwit hthealgorithmin[ 52 ], sincethesamemeasurementtypesareconsideredin[ 52 ].Theparameters e and k e fordistributionsofrelativeorientationandbearingmeas urementsofcamera 2 and 3 aregiveninTable 5-1 .However,thenoiseparametersformeasurementsobtainedb y camera 1 areallowedtovaryasfollows:fororientationmeasurement s, e =0.087 K andforbearingmeasurements, k e =20 K where K 2 [2 8 ,2 8 ] .ThewrappedGaussian distribution(forrelativeorientationmeasurements)and VonMises-Fisherdistributions (forrelativebearingmeasurements)producemoreorlessno isymeasurements dependingonthevalueof K .Forlargervaluesof K ,orientationmeasurementsbecome morenoisy,whilebearingmeasurementsbecomelessnoisy.F oreachvalueof K considered,noisymeasurementsaregeneratedfromthecorr espondingdistributions. ThesemeasurementsarethenusedbytheML-RPGOalgorithmand thealgorithm in[ 52 ]toestimatetheposeofeachcamera.Tocoincidewiththeass umptionsmade in[ 52 ],theinitialposeestimatesarenotusedasmeasurementsin theML-RPGO 102

PAGE 103

0.1 0.2 0.3 0.4 0.5 Noiseparameter(K) ML-RPGO ML-RPGO Algo.in[ 52 ] 2 8 2 4 2 0 2 4 2 8E d ( T ^ T ) (a) 0 0.02 0.04 0.06 0.08 Noiseparameter(K) ML-RPGO Algo.in[ 52 ] 2 8 2 4 2 0 2 4 2 8 var d ( T ^ T ) (b) Figure5-3.ComparisonofML-RPGOandthealgorithmproposed byTronandVidal in[ 52 ]forvaryinglevelsofnoiseintherelativemeasurementsfo ra3cameranetwork.Larger K correspondstonoisierrelativeorientation measurementsandlessnoisybearingmeasurements.Boththes ample(a) Biasand(b)varianceareshown. algorithm.Thiswillbereectedinourchoiceofdistanceme tric.Foreachmethodof estimationandeachvalueof K ,thebiasandvarianceofthedistancebetweenpose estimatesiscomputedfromaMonte-Carlosimulationwith 200 samples.Thedistance betweenapose T = f ( R i t i ) g ni =1 ofanetworkof n camerasanditsestimate ^ T is denedas d ( T ^ T ):= n X i =1 ( d 2 ( R i ^ R i )+ t i k t i k ^ t i k ^ t i k 2 ) 1 = 2 (5–11) Usingtheorientationandbearingmeasurementsalone,thep oseofallcamerascan onlybeestimateduptoascaleambiguity.Forthatreasonweh avenormalizedthe positionestimatestoadjustfortheambiguityinscale. TheresultsarereportedinFigure 5-3 .Thoughbothalgorithmsprovideaccurate estimates,weseethatwhenallmeasurementshaveequalamou ntofnoise,the algorithmin[ 52 ]provestobemoreaccurate.Thisislikelyduetoanaddition al optimizationstepfoundin[ 52 ]inwhichtheinitialestimatesareimprovedbyminimizing anadditionalcostfunction.TheML-RPGOalgorithmdoesnotp erformthisadditional 103

PAGE 104

step,thoughitcouldbeimplementedifdesirable.Howevert heML-RPGOalgorithm providesmoreaccurateestimateswhenthedifferencebetwe enthenoiselevelsin thevariousmeasurementsislarge.ThisoccurssincetheMLRPGOalgorithmtakes intoaccountthenoiseineachmeasurementinaprincipledwa ytocomputethemost likelyestimatesgiventhisinformation.Thisreducesthee ffectofmeasurementsthat arehighlynoisy,whileheighteningtheeffectsofmeasurem entswithlowernoise.No suchweightsarepresentinthealgorithmin[ 52 ].Whileitispossibletomodifythe costfunctionin[ 52 ]toincludeweights,duetothenon-Euclideannatureofthere lative orientationmeasurements,itisnotclearhowonewoulddete rminetheseweights. 5.3Summary Inthischapterwerstidentiedreasonabledistributions foreachoftheconsidered measurementtypes.Thedistributionswerechoseninsuchaw ayastodependon thetruevalueofthegivenmeasurement.Additionally,asinp reviouschaptersand incontrasttomuchofthepreviousworkinthisarea,theorie ntationsmeasurements weregiventheirnaturalmanifoldstructure,andsothecorr espondingdistributionwas denedoverthemanifold SO (3) withrespecttotheappropriateprobabilitymeasure. Anovelcollaborativelocalizationalgorithm,referredto astheML-RPGOalgorithm, wasthenproposedbasedonthemaximumlikelihoodestimator fortheabsolutepose ofeachoftherobots.Theresultingalgorithmwasgivenbymi nimizingthenegative log-likelihoodfunction.AsinChapter 3 thisresultsinanoptimizationproblemofthe productRiemannianmanifold. Simulationswerepresentedthatstudiedtheeffectofincrea singnumbersof neighborsonestimationaccuracy.Finally,simulationswe representedcomparingthe ML-RPGOalgorithmwithastateoftheartcameranetworklocal izationalgorithm. 104

PAGE 105

CHAPTER6 OUTLIERREJECTIONONPOSEGRAPHS InChapter 3 wepresentedanalgorithmforcollaborativelocalizationw hen inter-robotrelativemeasurementscanbeoftherelativepo sition,orientation,bearing, ordistance.Inthissection,werestrictourfocustoonlyme asurementsoftherelative orientationorpose(orientationandposition),andconsid erhowoutliersinmeasurements ofthesetypesmightbeidentiedandrejected. 6.1Problemstatement Considerthecollaborativelocalizationproblemasdescri bedinSection 3.1 withthe restrictionthatallinterrobotmeasurementsareoftherel ativepose.Thetimevarying measurementgraphisgivenby G ( k )=( V ( k ), E ( k )) .Notethatthelabelingfunction ` ( k ) (andthusthefullylabelednatureofthegraph)isunnecessa ryasallmeasurements areofthesametype.Let n ^ M e o e 2 E ( k ) denotethecorrespondingsetofinter-robotand inter-timerelativemeasurements.Anoutlierisameasureme nt ^ M e 0 2 n ^ M e o e 2 E ( k ) that isinconsistentwiththeremainingmeasurements.Anoutlier ismostoftenagrossly inaccuratemeasurementinasetofmeasurementsthatarerel ativelyaccurate(for whichthenoiseissmall).Insuchacase,if ^ M e 0 isanoutlier,weexpecttheestimate ofthenodevariablescorrespondingtothemeasurementgrap h ( V ( k ), E ( k ) nf e 0 g ) to bemoreaccuratethentheestimatescorrespondingto G ( k ) ,inwhichtheoutlierisstill included.Theoutlierrejectionproblemistoidentifyandr emovesuchoutliersusing onlytheinformationprovidedbythemeasurements,andnoin formationaboutthenode variables.Inparticular,anyknowledgeofapriordistribu tiononthevalueofthenode variablesisnotavailable. 6.2OutlierRejectionAlgorithm Considerasetofrobotsattemptingtoperformcollaborativ elocalizationusinga posegraphoptimizationCLalgorithminthepresenceofoutl ierswhenallmeasurements 105

PAGE 106

areoftherelativepose.Let G ( k )=( V ( k ), E ( k )) denotethecorrespondingmeasurement graphand n ^ M e o e 2 E ( k ) thesetofallinter-robotandinter-timerelativemeasurem ents. Foreachedge e 2 E ( k ) ,write e ( a b ) todenoteedge e isfromnode a tonode b Asimplecycleisanorderedcollectionofedges c =( e 0 e ` ) suchthat a i = b i 1 i =1, ` a 0 = b k a i 6 = a j 8 i 6 = j Wewillrefertothesetofallsuchcyclesinthegraph G ( k ) as C ( k ) .Forameasurement graphinwhichallmeasurementsareoftherelativepose,com posingnoise-free measurementsalonganycycleyieldstheidentity.Toutiliz ethisfact,forthesimple cycle c =( e 0 e ` ) 2 C ( k ) denethecyclemeasurement ^ M c = ^ M e 0 ^ M e 1 ^ M e ` .Since ^ M c = id 2 SE (3) wheneverthemeasurementsarenoise-free,asuitabledista ncemetric ontheproductmanifold ( SO (3) R 3 ) providesameasureofthenoiseencounteredin cycle c whenmeasurementsdocontainnoise.Towardsthisend,wede nethecycle consistencycost D C : C ( k ) R + as D C ( c 2 C ( k ))= q d 2 ( id SO (3) ^ R c )+ k ^ t c k 2 j c j (6–1) where ^ M c =( ^ R c ^ t c ) and d ( ) istheRiemanniandistance[ 62 ]givenby d ( A B )= r 1 2 Tr log 2 ( A T B ) A B 2 SO (3). While D C ( c ) providesameasureoftheaveragenoiseencounteredalongth esimple cycle c 2 C ( k ) ,using D C ( c ) alonelittlecanbesaidabouttheaccuracyofanyparticular measurement.Instead,wewillconsiderthesimplecyclecon sistencycostsforallcycles containingagivenedgeofinterest.Let C e ( k ) C ( k ) denotethesetofallcyclesthat 106

PAGE 107

includeedge e .Theedgeconsistencycost D : E ( k ) R isthendenedas D ( e 2 E ( k ))=min c 2 C e ( k ) f D C ( c ) g (6–2) D ( e ) providesameasureofthenoiseinmeasurement ^ M e basedonlyonthegiven measurementsconsistencywithothermeasurementsinthegr aph. Asthenumberofmeasurementsinagraph G ( k ) grow,ndingallcycles,and subsequentlythevalueof D ( e ) for e 2 E ( k ) ,canbecomeinfeasible.Instead,wewill willconsidersomesubsetfoundthroughadepthrstsearch( DFS)onthegraph[ 63 ]. Inparticular,let m indicatethenumberofedgesin E ( k ) anddeneatuningparameter M > 0 .Wethennd mM randomcyclesbyusingtheDFS.Thesetofcyclesfound inthismannerisdenotedby ^ C ( k ) C ( k ) .Wethendenetheapproximateedge consistencycost ^ D ( e 2 E ( k )) as ^ D ( e )=min c 2 ^ C e ( k ) f D C ( c ) g (6–3) where ^ C e ( k ) ^ C ( k ) isthesetofallcyclesfoundthatcontaintheedge e Forthealgorithmtoperformwell,thenumberofoutliers,th etopologyofthegraph, andthevalueofthetuningparameter M shouldbesuchthatthefollowingconditionis satised:If e 2 E ( k ) isnotanoutlier,thenthereexistsasimplecycle c 2 ^ C e ( k ) suchthat everyedgein c isnotanoutlier.Whenthisconditionissatised,ifalargev alueof D C ( c ) indicatesthepresenceofanoutlierinthecycle c ,thenweexpect ^ D ( e ) tobelargeifand onlyif e itselfisanoutlier. Toidentifyoutliersbasedontheedgeconsistencycosts,ah ypothesistestoftheset n ^ D ( e ) j e 2 E ( k ) o willbeutilized.Thoughthevaluesof ^ D ( e ) arenot i i d (independent, identicallydistributed)wewillmakethesimplifyingassu mptionthattheyinfactarei.i.d. Further,wechoosethelog-normaldistributiontodescribe theidenticaldistributionfor theedgeconsistencycostofeachedge.Evidencesupportingt hischoiceofdistribution willbepresentedinSection 6.4.1 .Finallytheset S ( k )= n log( ^ D ( e )) j e 2 E ( k ) o is 107

PAGE 108

considered.Underthesimplifyingassumptionthatthevalu es ^ D ( e ) aredistributed i i d log-normal,theset S ( k ) willbedistributed i i d normal.Theonesidedversion ofGrubbs'testforoutliers[ 64 ]canthenbeusedtoidentifylikelyoutliersinthe measurementsasfollows.Giventhedataset S ( k ) ,wesayavalue s 2 S ( k ) isanoutlier (indistribution)ifitisnotdistributedaccordingtothes ame i i d normaldistribution describingtheprobabilityofseeingtheothervaluesin S ( k ) .Thenullhypothesis, H 0 isthattherearenopositiveoutliersintheset S ( k ) .Here“positive”indicatesweare onlyconsideringoutlierstotherightofthemean.Forinsta nce,outliersthatarevery negativewouldnotberejected.Rejectingthenullhypothes isisequivalenttoaccepting thealternatehypothesis,whichstatesthatthelargestval uein S ( k ) isanoutlier(in distribution).TheonesidedGrubbs'teststatisticisgive nby G = s max s s where s max denotesthemaximumvaluein S ( k ) ,and s s arethesamplemeanand samplestandarddeviationof S ( k ) respectively.Thenullhypothesisisrejectedata signicancelevelof if G > N 1 p N vuut t 2 = N N 2 N 2+ t 2 = N N 2 where t = N N 2 denotestheuppercriticalvalueofthet-distributionwith N 2 degreesof freedomandasignicancelevelof = N Ifthenullhypothesisisrejected, s max isremovedfrom S ( k ) andthehypothesis testisrepeateduntilthenullhypothesiscannotberejecte d.Eachtimeanoutlier(in distribution)isremovedfromtheset S ( k ) ,theedgethatgeneratedthatparticularvalue isalsoremovedfromthegraph,thusdiscardingthemeasurem entasasuspected outlier.Remark6.1. Althoughtheoutlierrejectionalgorithmisdevelopedforth ecasewhen allmeasurementareoftherelativepose,extendingthealgo rithmtothecasewhenall 108

PAGE 109

measurementsareoftherelativeorientationinsteadisstr aightforward.Utilizingthefact thatnoisefreeorientationmeasurementscomposedovercyc lesalsoyieldtheidentity, thealgorithmaspresentedneedonlybemodiedbyredening thecycleconsistency costfunction D c as D c ( c 2 C ( k ))= d ( id SO (3) ^ R c ) j c j 6.3SlidingWindowApproximation Straightforwardapplicationofthemethoddescribedinthep revioussectionisonly possibleuptoacertaintime,beyondwhichthesizeofthegra phmakescomputations infeasible.Undersuchacondition,aslidingwindowapprox imationcanbeused.Sliding windowapproximationiscommonlyusedinbothposegraphCLa ndgraphSLAM(see [ 65 – 67 ]).Inthissectionwebrieyreviewtheslidingwindowappro ximationandprovide amodicationunderwhichtheslidingwindowapproximation canbeusedintheoutlier identicationproblem. Theslidingwindowmeasurementgraphattime k isgivenbyremovingall measurementsthatoccurredbeforetime k s .Oftenthiscutremovesalledgesleading tonode 0 ,resultinginadisconnectedgraph.Forexample,suchadisc onnectedgraph wouldeventuallyresultifallrobotshaveaccesstoGPSandco mpassmeasurements attime 0 ,butatnolatertime.Toreconnectthegraph,themostrecent nodevariable estimatesforthenodesintroducedattime k s (theearliestnodesstillinthegraph) areusedasmeasurementsbetweenthosenodesandnode 0 .Specically,thesliding windowmeasurementgraph G s ( k )=( V s ( k ), E s ( k )) where V s ( k )= V ( k ) n V k ( s +1) [f 0 g E s ( k )= E ( k ) n E k ( s +1) [f e =(0, j ) j j 2 V ( k s ) n V ( k ( s +1)) g 109

PAGE 110

andtheadditionaledges (0, j ) in E s ( k ) correspondtomeasurementsgivenbythe nodevariableestimates ( ^ R j ^ t j ) 2 n ( ^ R i ^ t i ) o V ( k 1) n V ( k ( s +1)) foundusingaposegraph optimizationcollaborativelocalizationalgorithm. Thedescriptionabovecorrespondstothestandardslidingw indowimplementation mostoftenseeninliterature.Whenusingtheslidingwindowa pproximationtoidentify outliers,asimplemodicationisnecessary.Let G s ( k )=( V s ( k ), E s ( k )) denotea slidingwindowposegraph.Ratherthenattemptingtoidenti fyoutliersin G s ( k ) ,a reducedgraph G r ( k )=( V r ( k ), E r ( k )) isused,where V r ( k )= V s ( k ) nf 0 g and E r ( k )= E s ( k ) nf (0, j ) j theedge (0, j ) correspondstoanestimateofthenodevariable, notatruemeasurement g .Insimpleterms,weremovetheadditionaledgesconnected to 0 addedintheconstructionoftheslidingwindowposegraph.T hisreductionofthe slidingwindowgraphisnecessaryas,forsufcientlylarge k ,themeasurements(really nodevariableestimates)correspondingtotheseedgesaree xpectedtobemorenoisy thentheinter-robotandinter-timemeasurements.Infact, inmanycasestheuncertainty inthesenodevariableestimateswillgrowwithoutboundwhi letheuncertaintyin inter-robotandinter-timemeasurementsremainsconstant .Finally,theoutlierrejection algorithmdescribedinSection 6.2 isnowappliedtothereducedslidingwindowgraph soconstructedateverytimeindex k .Theoriginal(non-reduced)slidingwindowgraph, minustheidentiedoutliers,canthenbepassedtoanyappro priateposegraphCL algorithm.Remark6.2. Thoughonlythecentralizedsolutionisdiscussedinthiswo rk,theoutlier rejectionalgorithmutilizingtheslidingwindowapproxim ationcanbedistributedinsuch awayastohaveeachrobotperformitsownoutlierrejectionu singonlymeasurements forneighboringrobots.Inparticular,eachrobotwillmain taincontactwithanyrobots ithasmeasured(orthathasmeasuredit)sincetime k s (werefertosuchrobots aneighbors)andconstructalocalslidingwindowmeasureme ntgraphconsistingof inter-robotmeasurementsbetweenitsselfanditsneighbor s,aswellasinter-time 110

PAGE 111

measurementsforitselfandforeachofitsneighbors.Outli errejectioncanthenbe performedonthelocalslidingwindowmeasurementgraph. 6.4Simulation Inthissectionwerstpresentresultsjustifyingtheuseof thelog-normaldistribution todescribetheedgeconsistencycostdistribution,whichi snecessaryintheapplication ofGrubbs'testforoutliers.TheninSection 6.4.2 wepresentsimulationsinwhich outliersarepresentduringcollaborativelocalizationan dtheresultsofapplyingthe outlierrejectionalgorithm.6.4.1Justicationofthelog-normaldistribution Aposemeasurementgraph G =( V E ) with 50 nodesand 202 edgeswasused intwosimulationsinwhichallmeasurementswereoftherela tivepose.Outlierswere generatedbycorrupting 3% oftherelativeposemeasurementsby 10m inpositionand 90 inorientation(aboutarandomaxis).Therobotsmovedatana veragespeedof .1m pertimestep,soa 10m errorisenoughtomakeameasurementanoutlier.The 90 errorintheorientationmeasurementwaschosenbecauseiti scommonlyseenwhen utilizingvision-basedsensors(see[ 68 ]).Theaverageangularspeedoftherobotswas 8 pertimestep,andsotheerrorinorientationisalsosufcie ntlylargetobeclassied asanoutlier.Thetunningparameters M and weresetat 10 and 0.025 respectively. Thevaluesof n ^ D ( e ) j e 2 E o werecomputedforthegraph G .Gaussiankernel densityapproximationwasthenusedtocomputeanestimateo fthepdfdescribing thevaluesfound.Inaddition,thesamplemeanandvariancew erecomputed,andthe log-normaldensitywiththeequivalentmeanandvariancewa sidentied. InFigure 6-1 boththeestimatedpdf,alongwiththecorrespondinglog-no rmal densityfunctioncanbeseen.Acomparisonoftheestimateda ndlog-normalpdf showsthat,whilenotanexactmatch,theapproximateshapei sadequatelycaptured. Mostimportantlytothesuccessofthehypothesistestbased algorithmpresentedin 111

PAGE 112

0 0.1 0.2 0.3 0.4 0 10 20 30 40 50 60 Outliers Log-Normalpdf Estimatedpdff ^ D ( e )^ D ( e ) Figure6-1.Acomparisonbetweentheestimatedpdffortheva luesof ^ D ( e ) ascompared tothecorrespondinglog-likelihooddistributionpdf.The locationsofthe outliersarealsoindicated. Section 6.2 ,thevalueof ^ D ( e ) when e isoutoutlierisalsoanoutlierfortheidentied log-normaldistribution.6.4.2OutlierRejection Wenowpresentasetofsimulationsthatprovidesomeinsight intotheeffectiveness oftheproposedalgorithm.Firstwedeneasetofconvenient performancemetrics.The positionestimationerrorofrobot i isdenedas e i ( k ):= ^ t i ( k ) t i ( k ) ,where t i ( k ) isits absolutepositionat k and ^ t i ( k ) istheestimate.Thebiasinthepositionestimationerror ofrobot i isdenedas k E[ e i ( k )] k ,where kk isthe2-normand E denotesexpectation. Thestandarddeviationisdenedas p Tr [ Cov ( e i ( k ), e i ( k )) ] ,where Cov ( ) stands forcovariance.Ineachscenariodescribedbelow,thebiasa ndstandarddeviationin positionestimationerrorisestimatedthroughtheuseofaM onteCarlosimulationwith 300 sampleruns. Agroupof 4 robotsweresimulatedtomovealongdistinct3-Dpaths.Twor obots wereabletoobtainnoisyrelativeposemeasurementsattime k iftheEuclideandistance betweenthematthattimewaslessthen 7m .TheRiemannianposegraphoptimization algorithmpresentedin[ 69 ]wasusedtoperformthecollaborativelocalizationin 112

PAGE 113

0 10 20 30 40 50 0 1 2 3 4 SelfLocalizationW/OutOutliersW/OutliersW/OutlierRejec. TimeBias(a) 0 10 20 30 40 50 0 2 4 6 8 10 SelfLocalizationW/OutOutliersW/OutliersW/OutlierRejec. Time StandardDeviation(b) Figure6-2.Simulationresultsstudyingtheeffectofoutlie rsandtheoutlierrejection algorithmonlocalizationaccuracy.Resultsareshownforc ollaborative localizationwithoutoutliers,withoutliersandnoreject ion,andwithoutliers whentherejectionalgorithmisutilized.The(a)Biasand(b) Standard Deviationinpositionestimationerrorareestimatedusing a300-iteration Monte-CarloSimulation. threecase.Intherstsimulation,allmeasurementswereeq uallyaccurate,thatis, nooutlierswerepresent.Inthesecondsimulation,outlier sweregeneratedbycorrupting approximately 4% ofthemeasurementsby 90 inorientation(aboutarandomaxis)and 10m inposition.Finally,thesamesetofoutlierinducedmeasur ementswasconsidered, buttheoutlierrejectionalgorithmpresentedinSection 6.2 wasutilizedtoidentifyand rejectlikelyoutliers.Forthissimulationtheslidingwin dowapproximationdescribedin Section 6.3 wasutilizedwithawindowlength( s )of 3 .Thetunningparameters M and weresetat 10 and 0.025 respectively.Thebiasandstandarddeviationofthepositi on estimationerror,denedabove,estimatedusinga300-iter ationMonte-Carlosimulation arepresentedinFigure 6-2 .Asummaryoftheaveragenumberofmeasurements, outliers,andfalsenegatives/positivesisreportedinTab le 6-1 AnumberofobservationscanbemadefromtheplotsinFigure 6-2 .Therst observationisthatthepresenceofoutlierscancausethees timatesfoundusing collaborativelocalizationtobecomeevenlessaccurateth anthosefoundusingdead 113

PAGE 114

Average Percentage Measurements 48.1 Outliers 1.85 3.7% FalseRejects 0.22 0.5% FalseAccepts 0.087 5% Table6-1.Theaveragenumberofmeasurements,outliers,me asurementsfalsely rejected,andoutliermeasurementsfalselyacceptedwhent heoutlier rejectionalgorithmissimulatedasapreprocessingstepof collaborative localization.Theaveragesarewithrespecttoall300Monte -Carlosimulations overall 50 timesteps.Thepercentagesarewithrespecttototal measurementsforOutliers,non-outliermeasurementsforf alselyrejected measurements,andoutliermeasurementsforfalselyaccept ed measurements. reckoningalone.Toseewhythisisso,itisimportanttonote thatthecollaborative localizationalgorithmusedinthissimulationisessentia llyaleastsquaresoptimization problem.Assuch,thealgorithmissensitivetooutliers,tot heextentthattheestimated nodevariablesmaybetterreecttheinformationcontained intheoutliers,ratherthen theremaining,moreaccuratemeasurements. ThesecondobservationtobemadefromFigure 6-2 istheconsiderableimprovement tolocalizationaccuracywhentheproposedoutlierrejecti onalgorithmisutilizedbefore performingcollaborativelocalization.Infact,accuracy isalmostasgoodasthatif nooutlierswerepresent.Thisisunsurprisingas,fromTabl e 6-1 itisevidentthatthe majorityofoutlierswerecorrectlyidentied,andveryfew additionalmeasurements werefalselyrejected.Asufcientnumberofrelativemeasu rements,approximately 96% remainedaftertheoutlierrejectionpreprocessing;enoug htoallowthecollaborative locationalgorithmtoperformwell. 6.5SummaryandFutureWork Inthischapterwepresentedanalgorithmtoidentifyandrej ectionoutliersasa preprocessingsteptoposegraphoptimizationbasedcollab orativelocalization.The algorithmutilizedpropertiesofrelativeposemeasuremen tscomposedovercycles 114

PAGE 115

todevelopametriconthesetofedgesindicativeoftheprese nceofanoutlier.A hypothesistestwasthenutilizedtoidentifythelikelyset ofoutliers. Simulationswerepresentedthatstudiedtheeffectivenesso ftheoutlierrejection algorithmwhenutilizedbeforeperformingcollaborativel ocationonasetofrobots.Itwas shownthat,whilethepresenceofoutlierscancausecollabo rativelocalizationtoperform evenworsethendeadreckoning,theoutlierrejectionalgor ithmsucceedsinthetaskof removingoutlierstosuchaextentthatperformanceisincre asednearlytothatofthe no-outliercase. Inaddition,simulationswerepresentedthatexploredthev alidityoftheassumption thatedgeconsistencycostswerei.i.d.log-normal.Thecri ticalconclusionofthis simulationwasthat,ifanedge e 0 isanoutlier,thecorrespondingapproximateedge consistencycost ^ D ( e 0 ) isanoutlierofthelog-normaldistributionwithmeanand variancegivenbythesamplemeanandvarianceoftheset n ^ D ( e ) o .Becauseofthis,we expecttocorrectlyidentifyoutlyingmeasurementsusingG rubbs'testforoutliers. 115

PAGE 116

CHAPTER7 CONCLUSIONANDFUTUREWORK 7.1Conclusion Inthisdissertationwepresentedworkconsideringbothsin glerobotlocalization andmulti-robotcollaborativelocalization.InChapter 2 westudiedthegrowthoferror inpositionestimateswhendeadreckoningisusedtolocaliz earobotwithoutaccessto GPS.Weexaminedthegrowthoferrorinpositionestimatesobt ainedfromnoisyrelative posemeasurements.Weshowedthatinboth2-Dand3-D,thebia sandthevarianceof thepositionestimationerrorgrowsatmostlinearlywithti meordistancetravelled.The precisegrowthrateofthebiasdependsonthetrajectoryoft herobot.Specically,ifthe robotstaysinaboundedregion,thebiasisupperboundedbya constantforalltime.It wasprovedthatthevariancegrowthrateisalsolowerbounde dbylinearfunctionoftime ifthetranslationmeasurementerrorsarelargeenough.Exac tformulasfortheerrorbias andvariancewereobtainedfortwospecial2-Dtrajectories ,straightlineandperiodic. ExtensiveMonte-Carlosimulations,andexperimentswithaw heeledrobot,wereusedto verifytheresults. Theresultsofthischaptershowthatlocalizationerrorgro wthrateisnotsuperlinear withtimeordistanceevenwithoutabsoluteorientationsen sors.Inaddition,itturnsout thattheasymptoticgrowthrateofthebiasdoesnotchangeev enifallthemeasurements areunbiasedorevenifthetranslationmeasurementsarecom pletelyerrorfree.The biasgrowthisprincipallyduetothefactthattheexpectedv alueoftheestimated positionconvergestoapoint,irrespectiveofhowtherobot ismoving.Thisoccurssince r ,thenormoftheexpectedrotationerror,isstrictlylessth anunity.Asaresult,the magnitudeofthemeasuredtranslation,oncethemeasuremen tistransformedtothe globalcoordinateframe,decaysgeometricallywithtime. 116

PAGE 117

InChapter 3 weintroducedanoveldistributedalgorithmforestimating the3-D poseofmultiplerobots,referredtoastheRPGOalgorithm.Th ealgorithmutilizingnoisy inter-robotmeasurementsofvarioustypes(relativepose, orientation,position,bearing, ordistance)betweenpairsofrobots,whenavailable,along withthenoisyinter-time relativeposemeasurementsusuallyutilizedfordeadrecko ning.Thedistributed algorithmisinspiredbyacentralizedalgorithmforsolvin galeast-squarestype probleminwhichthenaturalmanifoldstructureofthespace ofrotationsisutilized.The least-squareslikecostfunctionischosentomeasurehowwe lltheestimatesexplain therelativemeasurements.Agradient-descentinaproduct Riemannianmanifoldis usedtosolvetheoptimizationproblemensuringtheestimat esremainonthemanifold withouttheneedforanyprojection.Theproposedalgorithm doesnotrelyonany particularparameterizationoftheunderlyingmanifold;a llowinganyparameterization tobeusedinnumericalcomputationswithoutaffectingther esultingestimates.The algorithmisprovablycorrectinthesensethatthesolution convergestoacriticalpointof thecostfunctionasthenumberofiterations(gradientdesc entsteps)increases.Then inChapter 4 ,twostateoftheartcollaborativelocalizationalgorithm swerecompared withtheRPGOalgorithm.Therstalgorithm,referredtoasth eEuclideanPoseGraph Optimizationalgorithm,wasaclassicalleastsquaresbase dposegraphoptimization algorithm.Thesecondalgorithmwasbasedonaindirectexte ndedKalmanlter.In eachcase,simulationswerepresentedthatidentiedsitua tionsunderwhichtheRPGO providedmoreaccurateposeestimates. InChapter 5 weidentiedreasonabledistributionsforeachoftheconsi dered measurementtypes.Thedistributionswerechoseninsuchaw ayastodependon thetruevalueofthegivenmeasurement.Additionally,asinp reviouschaptersand incontrasttomuchofthepreviousworkinthisarea,theorie ntationsmeasurements weregiventheirnaturalmanifoldstructure,andsothecorr espondingdistributionwas denedoverthemanifold SO (3) withrespecttotheappropriateprobabilitymeasure. 117

PAGE 118

Anovelcollaborativelocalizationalgorithm,referredto astheML-RPGOalgorithm, wasthenproposedbasedonthemaximumlikelihoodestimator fortheabsolutepose ofeachoftherobots.Theresultingalgorithmwasgivenbymi nimizingthenegative log-likelihoodfunction.AsinChapter 3 thisresultsinanoptimizationproblemofthe productRiemannianmanifold.Simulationswerepresentedth atstudiedtheeffectof increasingnumbersofneighborsonestimationaccuracy.Fi nally,simulationswere presentedcomparingtheML-RPGOalgorithmwithastateofthe artcameranetwork localizationalgorithm. Finally,inChapter 6 wepresentedanalgorithmtoidentifyandrejectionoutlier sasa preprocessingsteptoposegraphoptimizationbasedcollab orativelocalization(suchas thealgorithmpresentedinChapter 3 ).Thealgorithmutilizedpropertiesofrelativepose measurementscomposedovercyclestodevelopametriconthe setofedgesindicative ofthepresenceofanoutlier.Ahypothesistestwasthenutil izedtoidentifythelikelyset ofoutliers. Simulationswerepresentedthatstudiedtheeffectivenesso ftheoutlierrejection algorithmwhenutilizedbeforeperformingcollaborativel ocationonasetofrobots.Itwas shownthat,whilethepresenceofoutlierscancausecollabo rativelocalizationtoperform evenworsethendeadreckoning,theoutlierrejectionalgor ithmsucceedsinthetaskof removingoutlierstosuchaextentthatperformanceisincre asednearlytothatofthe no-outliercase. Inaddition,simulationswerepresentedthatexploredthev alidityoftheassumption thatedgeconsistencycostswerei.i.d.log-normal.Thecri ticalconclusionofthis simulationwasthat,ifanedge e 0 isanoutlier,thecorrespondingapproximateedge consistencycost ^ D ( e 0 ) isanoutlierofthelog-normaldistributionwithmeanand variancegivenbythesamplemeanandvarianceoftheset n ^ D ( e ) o .Becauseofthis,we expecttocorrectlyidentifyoutlyingmeasurementsusingG rubbs'testforoutliers. 118

PAGE 119

7.2FutureWork Throughoutthelaterchaptersofthiswork,thereexistedth eimplicitunderlying assumptionthatrobotswerecapableofbeinguniquelyident ied.Thoughthisisa commonassumptionintheliteratureoncollaborativelocal ization,itisnoteasyto achieveinpractice.Itmaybepossibletousetheinter-robo tmeasurementstoidentify arobot,asdonein[ 33 ].Astudyofhowsuchmethodsmightbeutilized,andhowthose methodswillaffectthelocalizationestimatesisanareaof futurestudy. InChapter 5 ,statisticalinformationabouttherelativemeasurements wasutilized todevelopamaximumlikelihoodestimator.Oneinteresting areaoffutureworkisto furtherinvestigatethevalidityofthechosendistributio nsthroughtheuseofhypothesis testingonexperimentaldata.Anothercriticalareaoffutur estudyisthatofpropagating thestatisticalinformation.Thatis,identifyingthedist ributionsoftheposeestimates aftertheML-RPGOalgorithmhasbeenutilized.Oncethatprop agationstephasbeen considered,theML-RPGOalgorithmcanbedistributedinmuch thesamewasasthe RPGOalgorithmpresentedinSection 3.3 Thesuccessoftheoutlierrejectionalgorithmpresentedin Chapter 6 ishighly dependenttopologyoftheunderlyingmeasurementgraph.In particular,whileitisclear thatthenumberofedgesaffecttherobustnessoftheoutlier rejectionalgorithm,an in-depthstudyoftheconnectionbetweenthemaximumnumber ofoutliersforwhichthe rejectionalgorithmsucceedsversusthetopologyofthemea surementgraphremains anactiveareaforfuturework.Futurestudieswillalsoatte mpttoextendtheproposed outlierrejectionschemetoothermeasurementtypes:posit ion,bearing,anddistance. Finally,inadditiontothecentralizedalgorithmpresente dinSection 6.2 ,amethod todistributedtheoutlierrejectionalgorithmwasalsobri eyoutlinedinRemark 6.2 .In suchadistributedscheme,eachrobotposesadifferentloca lmeasurementgraphthat isonlyasmallpartofthetotalgraph.Howtheoutlierreject ionalgorithmperformson 119

PAGE 120

theselocalgraphsascomparedtothefullcentralizedsolut ionpresentedhererequires furtherstudy. 120

PAGE 121

APPENDIXA SINGLEROBT:PROOFS ProofofProp. 2.1 Let y bea d -dimensionalrandomvector.Since Cov ( y y )=E[ yy T ] E[ y ]E[ y ] T ,wehaveupontakingthetraceofbothsides k E[ y ] k 2 =E[ k y k 2 ] Tr [ Cov ( y y ) ] E[ k y k 2 ], since Tr [ Cov ( y y ) ] 0 .Moreoverequalityintheaboveinequalityholdsifandonly if thevarianceofeachofthecomponentsof y is 0 ,thatis, y isdegenerate.Wenowapply thisresulttotherandomvector y := R x ,where x isadeterministic d -dimensionalvector while R isarandomrotationmatrix: k E[ R ] x k 2 E[ k R x k 2 ]=E[ k x k 2 ]= k x k 2 (A–1) wheretherstequalityisduetothefactthatrotationdoesn 'tchangethe 2 -normof avector,andthesecondequalityisdueto x beingdeterministic.Thisprovesthat k E[ R ] k 1 .Since y isdegenerateifonlyif R is,theinequalityin( A–1 )isstrictif R is non-degenerate.Thisprovestheresult. Thefollowingadditionaltechnicalresultisneededforthe proofofLemma 1 PropositionA.1. If X i isasequenceofrandomvectorssuchthat E[ X T i X j ] 0 j i j j where j j < 1 and 0 isanarbitraryconstant,then E[( n X i =1 X i ) T ( n X i =1 X i )] 0 1+ 1 n Ifinaddition 0 j i j j E[ X T i X j ] for i 6 = j and 0 < 0 E[ X T i X i ] ,where 0 0 are constantssuchthat 0 > 2 j 0 j 1 j j ,then E[( P ni =1 X i ) T ( P ni =1 X i )]=( n ) ProofofProp. A.1 Expandingthesum,weobtain E[( n X i =1 X i ) T ( n X i =1 X i )]= n X i =1 T i (A–2) 121

PAGE 122

where T i := n X j =1 E[ X T i X j ]. (A–3) Itfollowsfrom( A–3 )andthehypothesisthat T i 0 ( i 1 + i 2 + + +1+ + + n i ) = 0 ( 1+ i 1 X k =0 k + n i X k =0 k ) 0 ( 1+ 1 X k =0 j j k + 1 X k =0 j j k ) = 0 ( 1+ 1 1 j j + 1 1 j j )= 0 1+ j j 1 j j wherethesecondinequalityfollowsfrom j j < 1 .Theupperboundnowfollows from( A–2 ).Thisprovestherststatement. Whentheadditionalhypothesisholds,wehave T i 0 ( i 1 + i 2 + + )+ 0 + 0 ( + + n i ) 2 j 0 j 1 X k =0 j j k + 0 = 0 2 j 0 j 1 j j =: ` 0 > 0 Itfollowsfrom( A–2 )that E[( P ni =1 X i ) T ( P ni =1 X i )] n ` 0 =n( n ) .Combiningthe asymptoticlowerandupperbounds,weget E[( P ni =1 X i ) T ( P ni =1 X i )]=( n ) ProofofLemma 1 Itfollowsfrom( 2–4 )that E[ ^ t 00, n ]= n X k =1 E[ ^ t 0k k +1 ] (A–4) From( 2–2 )-( 2–3 )weget ^ t 0k k +1 = R 01 ~ R 01 ... R kk +1 ~ R kk +1 t k +1 k k +1 + ~ t k +1 k k +1 ) E[ ^ t 0k k +1 ]= R 0k R ... R kk +1 R t k +1 k k +1 + k +1 122

PAGE 123

wherethesecondequalityfollowsfromtheassumptionthatt heorientationmeasurement errorsarei.i.d.Sincearotationdoesnotchangethe 2 -normofavector, k E[ ^ t 0k k +1 ] kk R k k k R kk t k +1 k k +1 k + k k +1 k wheretheinequalityfollowsfromapplyingtriangleinequa lityandusingsub-multiplicative propertyofinducednorms.Since k R k kk R k k ,weobtainuponusingProposition 2.1 andthedenition r = k R k that k E[ ^ t 0k k +1 ] k r k a where a :=sup k ( k R kk t k +1 k k +1 k + k k +1 k ) r + .Applyingtriangleinequalityto( A–4 ), weget k E[ ^ t 00, n ] k n 1 X k =0 k E[ ^ t 0k k +1 ] k a n 1 X k =0 r k a 1 r n 1 r since 0
PAGE 124

Wenowevaluatetheexpectedvaluesofthesefourterms.Byusi ngtheIndependenceof theorientationmeasurementerrorsand,weget E[ V 1 ]=( t i +1 i i +1 ) T R i +1 i +2 R ... R jj +1 R t j +1 j j +1 )j E[ V 1 ] jk t i +1 i i +1 kk R j i t j +1 j j +1 k k R j i kk t i +1 i i +1 kk t j +1 j j +1 k r j i 2 wheretherstinequalityusesthefactthatrotationsdonot changethe 2 -norm.For V 2 since ~ t i +1 i i +1 isstatisticallydependentonlyon ~ R ii +1 andnoton ~ R i +1 i +2 ... ~ R jj +1 ,itisalso independentof ^ R i +1 j +1 .Hence, j E[ V 2 ] j = j b i R i +1 i +2 R R jj +1 R t j +1 j j +1 )j E[ V 2 ] j r j i b Similarly,wehave,for i < j E[ V 3 ]=( t ii +1, i +1 ) T R i +1 j +1 R R j 1 j R j +1 )j E[ V 3 ] j r j i 1 r andfor i = j j E[ V 3 ] j b .For V 4 ,when i < j ,wehave V 4 =( ~ t i +1 i i +1 ) T R i +1 i +2 ~ R i +1 i +2 ... R jj +1 ~ R jj +1 ~ t j +1 j j +1 )j E[ V 4 ] jk b i kk R k j i 1 k j +1 k r j i 1 r b When i = j ,wehave V 4 =( ~ t j +1 j j +1 ) T ~ t j +1 j j +1 ,whichimplies E[ V 4 ]=Tr [ P j +1 ] + b Tj +1 b j +1 ,by denition.Therefore, 0 < p E[ V 4 ] p + b 2 .( i = j ). Combiningallfourterms,weget, 0 r j i E[( ^ t 0i i +1 ) T ^ t 0j j +1 ] 0 r j i ,( i < j ) 0 E[( ^ t 0i i +1 ) T ^ t 0i i +1 ] 0 124

PAGE 125

where 0 := ( 2 + b + 1 r + 1 r b ) 0 := 2 + b + 1 r + 1 r b ,and 0 := p ( 2 +2 b ) 0 := 2 +2 b + p + b 2 .Notethatincase 0 isnegative,itisapoorlowerlowerbound since ( ^ t 0i i +1 ) T ^ t 0i i +1 > 0 .Repeatingtheseargumentsfor i j andcombining,wendthat 0 r j i j j E[( ^ t 0i i +1 ) T ^ t 0j j +1 ] 0 r j i j j ,( i 6 = j ) max f 0, 0 g E[( ^ t 0i i +1 ) T ^ t 0i i +1 ] 0 where 0 :=max f 0 0 g .Nowcall X i := ^ t 0i i +1 ,sothat ^ t 00, n = P n 1 i =0 X i .Hence, E[( ^ t 00, n ) T ^ t 00, n ]=E[( P n 1 i =0 X i ) T ( P n 1 j =0 X j )] .ItnowfollowsfromProposition A.1 that E[( ^ t 00, n ) T ^ t 00, n ] is O ( n ) ,andis ( n ) if 0 > 2 j 0 j 1 r .Since j 0 j = 2 + b + ,thecondition 0 > 2 j 0 j 1 r isequivalentto p > 2 b + 2 +2 ( + =r )( + b ) 1 r ,whichprovestheresult. Proofof 2.2 Deneanewrandomvariable, ~ k 1, k := ~ k 1, k E[ ~ k 1, k ] .Then f ~ k 1, k g 1k =0 isani.i.d.sequenceandthemarginaldensityof ~ k 1, k issymmetric about 0 .Wedenethecorrespondingrotationmatrices ~ R ij := f R ( ~ i j ) .Utilizingthe commutativepropertyof2-Drotationmatrices,wehave ~ R ij = R j i ~ R ij .Itthenfollows from( 2–5 )that e ( n )= n r ^ t 00, n andfrom( 2–4 ),( 2–3 ),and( 2–2 )that ^ t 00, n = n X k =1 k Y i =1 R ~ R i 1 i r + ~ t kk 1, k wherewehaveusedthefactthat ^ R i 1 i = R i 1 i ~ R i 1 i = R ~ R i 1 i since R i 1 i = I duetothe natureofthetrajectory.Wedenetwonewrandomvariables f n := n X k =1 k Y i =1 R ~ R i 1 i r g n := n X k =1 k Y i =1 R ~ R i 1 i ~ t kk 1, k 125

PAGE 126

sothat ^ t 00, n = f n + g n (A–5) Bythei.i.d.assumptiononthesequence f ~ k 1, k g k ,thesequence f ~ R k 1 k g k isalsoi.i.d., sothat E[ ~ R ij ]=E[ k Y k = i +1 ~ R k 1 k ]= j Y k = i 1 E[ ~ R k 1 k ]= c j i I (A–6) wherewehaveusedthefactthat E[sin ~ i 1, i ]=0 ,whichfollowsfromAssumption 2.1 .It isthenstraightforwardtoshowthat E[ f n ]= n X k =1 ( c R ) k r = ( I c R ) 1 ( I ( c R ) n ) c R r E[ g n ]= n 1 X k =0 ( c R ) k = ( I c R ) 1 ( I ( c R ) n ) Theexpectedvalue e ( n ) isnow E[ e ( n )]= n r ( I c R ) 1 ( I ( c R ) n )( c R r + ) (A–7) whichprovestherstequalityin( 2–18 ). Forthevariance,itfollowsfrom( A–5 )that Tr [ Cov ( e ( n ), e ( n )) ] =Tr Cov ( ^ t 00, n ^ t 00, n ) =E[ f T n f n ]+E[ g Tn g n ]+2E[ f T n g n ] E[ ^ t 00, n ] T E[ ^ t 00, n ]. (A–8) 126

PAGE 127

E[ f T n f n ]= r T E 24 n X i =1 i Y j =1 R ~ R j 1 j T n X k =1 k Y ` =1 R ~ R ` 1 ` 35 r = r T h I + c R T + +( c R T ) n 1 + c R + I + c R T + +( c R T ) n 2 + ( c R ) n 1 + + I i r wherewehaveusedtheindependenceofthesequence f ~ R k 1 k g k andthefactthat ~ R k 1 k ~ R k 1 k = I = R R T .Theexpressionabovesimpliesto E[ f T n f n ]= r T nI +2 n 1 X k =1 ( n k ) ( c R ) k # r = r T ( I c R ) 2 I +2( n 2) c R 2( n 1) ( c R ) 2 +2 ( c R ) n +1 r Toexamine E[ g Tn g n ] ,weexpresstheproductas g Tn g n = P nk =1 T k where T k =( ~ t kk 1, k ) T ( ~ R k 1 k ) T ( ~ R k 2 k 1 ) T ...( ~ R 01 ) T ( R k ) T R ~ R 01 ~ t 10,1 + + R n ~ R 01 ... ~ R k 1 k ~ t kk 1, k Takingexpectationandusingtheassumptionsonthenoiseco rrelations,wegetfor k > 1 E[ T k ]=Tr P + bb T + b T (( c R ) k 2 +( c R ) k 3 + + I + I +( c R )+( c R ) 2 + +( c R ) n 1 k ) andfor k =1 E[ T k ]=Tr P + bb T + b T ( I + c R +( c R ) 2 + +( c R ) n 1 k ) .Repeating thisforallthe T k 'sweget: E[ g Tn g n ]= n Tr P + bb T + b T 2 n 2 X k =0 ( n k 1) ( c R ) k # = n Tr P + bb T + b T ( I c R ) 2 [ 2( n 1) I 2 nc R +2 ( c R ) n ] 127

PAGE 128

Similartediouscalculationsleadtothefollowing E[ f T n g n ]= n 1 X k =0 b T ( c R ) k + n 2 X k =0 ( n k 1) T ( c R T ) k # r = b T ( I c R ) 2 I c R ( c R ) n + ( c R ) n +1 r + r T ( I c R ) 2 [ ( n 1) I nc R +( c R ) n ] Pluggingallofthisbackin( A–8 ),weget Tr [ Cov ( e ( n ), e ( n ) )] = n + ( n ) ,where ( n ) aregivenin( 2–15 ).Thisprovesthesecondequalityin( 2–18 ). ProofofThrm. 2.3 Deneanewrandomvariable, ~ k 1, k := ~ k 1, k E[ ~ k 1, k ] .The sequence f ~ k 1, k g 1k =0 aretheni.i.d.andthemarginaldensityof ~ k 1, k issymmetric abouttheoriginforeach k .Wedenethecorrespondingrotationmatrices ~ R ij := f R ( ~ i j ) .Utilizingthecommutativepropertyofrotationsin2-D,we havethefollowing relation ~ R ij = R j i ~ R ij (A–9) Toexaminethebias,werstre-writethepositionestimate ^ t 00, n as ^ t 00, n = n X i =0 ^ t 0i i +1 = 1 X k =0 p X m =1 ^ t 0kp + m 1, kp + m + q X j =1 ^ t 0 p + j 1, p + j (A–10) wherethersttermissumisoveralltimestepsuptotheendof thelast( -th)period andthesecondtermforthetimestepsafterthat.Forany 0 m < p ,wehave ^ t 0kp + m 1, kp + m = ^ R 0kp + m ^ t kp + m kp + m 1, kp + m = R 0m ~ R 0kp + m ( t mm 1, m + ~ t kp + m kp + m 1, kp + m ), 128

PAGE 129

whereapartfrom ^ R = R ~ R ,wehaveusedtheperiodicnatureofthetrajectorythatlead s to R 0kp + m = R 0m and t kp + m kp + m 1, kp + m = t mm 1, m .Takingexpectationandusing( A–9 ),weobtain E[ ^ t 0kp + m 1, kp + m ]= R 0m ( c R ) kp + m 1 ( c R t mm 1, m + m ) Thisexpressionisusedtoevaluate E[ t 00, n ] bytakingexpectationoftherighthandside of( A–10 ).Aftergroupingterms,weobtain E[ t 00, n ]= 1 X k =0 ( c R ) kp ( p ) +( c R ) p ( q ) (A–11) UsingtechniquessimilartothoseusedintheproofofTheore m 2.2 ,itcanbeshown that E[ ^ t 00, n ]= 1 X i =0 ( c R ) ip w +( c R ) p w ( q ) ) E[ e ( n )]= q 1 X k =0 R 0k +1 t k +1 k k +1 1 X k =0 ( c R ) kp w ( c R ) p w ( q ). Byreplacingthesummationwearriveat( 2–20 ). 129

PAGE 130

APPENDIXB PRODUCTRIEMANNIANMANIFOLDS B.1RiemannianManifolds Inthissectionweprovidegreaterdetailonthebuildingblo cksofAlgorithm 1 includingRiemannianmanifolds.Afullintroductiontothe studyofRiemanniangeometry isoutsidethescopeofthiswork;theinterestedreaderisre ferredto[ 54 62 ]. Amanifold M ofdimension d isatopologicalspacethatcanlocallyberepresented by R d .Thesetofalllinearoperatorsonthevectorspace R 3 isdenotedby L ( R 3 ) Elementsof L ( R 3 ) canberepresentedby 3 3 matrices.Wecanthendescribethe manifoldof3-Drotations,denoted SO (3) ,bytheset R 2 L ( R 3 ): R T R = id det ( R )=1 Thesymbol id denotestheidentityoperator.Givenapoint p 2 SO (3) ,thetangentspace of SO (3) at p ,denoted T p SO (3) isgivenby T p SO (3)= p ^ v :^ v 2 L ( R 3 ),^ v T = ^ v A Riemannianmetric onamanifold M isgivenbydening 8 p 2 Mg p : T p M T p M R suchthat 8 p 2 M g p isaninnerproduct giventwovectorelds X and Y on M ,themap g : M R p 7! g p ( X p Y p ) is smooth. TheRiemannianmetric g givesrisetoaninner-productnormonthetangentspace T p M foreach p 2 M .Thatis,for 2 T p M k k 2 = g p ( ) .A Riemannianmanifold isa manifoldequippedwithaRiemannianmetric.TherstRieman nianmanifoldweconsider is ( SO (3), g ) weregistheRiemannianmetricgivenby g p ( A B )= 1 2 Tr A T B (B–1) forall p 2 SO (3) A B 2 T p SO (3) .Fromthispointon,whenwereferto SO (3) we meanthisRiemannianmanifold.Tofurthersimplifythenota tion,whentheargument p 2 SO (3) isclear,wewilldenote g p ( X p Y p ) by g ( X p Y p ) .ForaRiemannianmanifold, 130

PAGE 131

theanalogtoastraightlineinEuclideanspaceisgivenbya geodesic .Fortwopoints p and q onamanifold M ,ageodesicfrom p to q ,denoted r pq ,isa“shortest”pathfrom p to q .Moreprecisely,ifweconsiderthesetofallparameterized pathsfrom p to q ,given by = f r :[0,1] M : r (0)= p r (1)= q g thenageodesic r pq isgivenbyapaththat minimizes Z 1 0 k r 0 ( t ) k dt overallpathsin ,and r 0 ( t ) isthederivativeof r ( t ) withrespectto t ,whichisdened bythefollowingrelationship: r 0 ( t 0 ) f = d ( f ( r ( t )) dt j t 0 forall f : M R .Notethatthederivative r 0 (0) isanelementofthetangentspace T r (0) M .Forthemanifold SO (3) ,thegeodesicfrom p to q ( 2 SO (3) )isgivenby r pq ( t )= p exp( tv ), t 2 [0,1] where v =log( p 1 q ) ,themap exp: L ( R 3 ) L ( R 3 ) isgivenby exp( X )= P 1k =0 X k k and logistheinversemapofexp 1 ForanarbitraryRiemannianmanifold,theRiemannianmetri cdenesacorresponding distancefunctiononthemanifoldasfollow.ForaRiemannia nmanifold M ,thedistance betweentwopoints p q 2 M isgivenby d M ( p q )= Z 1 0 k r pq ( t ) k dt 1 Thisuseof log( p ) for p 2 SO (3) isonlyvalidontheregionwhere exp isa diffeomorphism,whichitisatleastontheopenballabout I 2 SO (3) ofradius [ 70 ]. 131

PAGE 132

Inparticular,forthemanifold SO (3) d SO (3) ( p q )= r 1 2 Tr log 2 ( p T q ) Thesubscript SO (3) onthedistancefunctionwillbeomittedwhenthearguments makeitclearwhatmanifoldthedistancerefersto.Thesecon dmanifoldweconsider is ( R 3 < > ) ,standardEuclidean3-spacewiththeRiemannianmetricgive nbythe innerproducton R 3 .Givenpoints p q 2 R 3 the(unique)geodesicconnecting p and q is thestraightlinefrom p to q and d ( p q )= k p q k := < p q p q > 1 = 2 ,thestandard Euclideaninnerproductnorm. Inadditiontothedistancefunction,theRiemmanianmetric alsospeciesaparallel transportfunction.Let M beanarbitraryRiemmanianmanifold.For p 2 M and 2 T p M the paralleltransport function,denoted exp p ,ndsanewpoint q 2 M givenbymoving alongageodesicbeginningat p andwithinitialvelocity .Moreprecisely, q =exp p ( )= r (1) where r isaparameterizedgeodesicwith r (0)= p and beingthetangentto r ( t ) at t =0 .Inthecaseof SO (3) exp p ( )= p exp( p T ) Finally,weconsiderthegradientforrealvaluefunctionsd enedonthemanifold. GivenaRiemannianmanifold M andascalar,real-valuedfunction f : M R ,the gradientof f at p 2 M ,denotedby gradf ( p ) ,canbedenedbythefollowingrelation: g p ( gradf ( p ), p )=( f r ) 0 (0) 8 p 2 M (B–2) where r :[0,1] M isanycurvewith r (0)= p ,and ( f r ) 0 (0):= d ( f r )( t ) dt j 0 .Foran exampleofagradientcalculationthatwillbeusefullater, considerthefollowingfunction f 1 : SO (3) R p 7! 1 2 d 2 ( p q ) (B–3) 132

PAGE 133

forsomexed q 2 SO (3) .Foranarbitrary p 2 SO (3) ,weconsiderthegeodesic r pq ( t )= p exp( tv ) where v = log ( p 1 q ) .Itcanbeshownthat ( f 1 r )( t )= 1 2 (1 t ) 2 d 2 ( p q ) andthus ( f 1 r ) 0 (0)= d 2 ( p q )= 1 2 Tr v 2 Thecorrespondingtangentvectorto r ( t ) at p isgivenby p := r 0 (0)= pv .Itfollows from( B–1 )that g p ( gradf 1 ( p ), p )= 1 2 Tr ( gradf 1 ( p )) T pv Applying( B–2 )wehave gradf 1 ( p )= pv = p log( p 1 q ) 8 p 2 SO (3). (B–4) Forafunction f : SO (3) R thegradientat R 2 SO (3) isgivenby[ 68 ]: gradf ( R )= f R R f T R R (B–5) where f R 2 L ( R 3 ) isthelinearoperatorwhosematrixrepresentation(usingt hecanonical basisvectorsfor R 3 )isgivenby: ( f R ) ij = @ f @ R ij ,where R ij representsthe ( i j ) -thentryof the 3 3 matrixrepresentationof R .Gradientcalculationforafunction f : R 3 R is straightforward. Apartfrom SO (3) and R 3 ,theotherRiemannianmanifoldthatisusefulinthisstudy is SE (3) .Insteadof SE (3) ,however,weconsidertheequivalentmanifold SO (3) R 3 ThereexistsanaturalwaytodenetheRiemannianmetricont hismanifold,based ontheRiemannianmetricsin SO (3) and R 3 sothatwecandealwithgeodesicsand gradients.Thishastodowiththefactthat SO (3) R 3 isaproductmanifold,whichisthe topicofthenextsection. 133

PAGE 134

B.2ProductManifold Let ( M i g ( i ) ) ni =1 beasetof n Riemannianmanifolds.Wedenetheproduct Riemannianmanifold ( M g ) asfollows: M := M 1 M n g p ( ):= n X i =1 g ( i ) p i ( i i ) (B–6) forall p =( p 1 ,..., p n ) 2 M andall =( 1 ,..., n ), =( 1 ,..., n ) 2 T p M .Werst restrictourconsiderationstoproductRiemannianmanifol dsoftheform M = M 1 M 2 (i.e. n =2 ).Theextensiontoanynitecombinationisstraightforwar d.Thefollowing lemmawillbeusefulinthesequel.Theproofofthelemmacanb efoundin[ 68 ]. Lemma2 ([ 68 ]) Let M = M 1 M 2 beaproductRiemannianmanifoldasdened in ( B–6 ) andconsiderasmoothfunction f : M R .Thenforanyparameterizedpath r =( r 1 r 2 ):[0,1] M d dt f ( r ( t )) j t 0 = d dt f ( r 1 ( t ), r 2 ( t 0 )) t 0 + d dt f ( r 1 ( t 0 ), r 2 ( t )) t 0 where t 0 2 [0.1] TheoremB.1. Let M = M 1 M 2 beaproductRiemannianmanifold,asdenedin ( B–6 ) Considerasmoothfunction f : M R andforall ( p q ) 2 M ( p 2 M 1 q 2 M 2 ),dene f q 1 : M 1 R a 7! f ( a q ) f p 2 : M 2 R b 7! f ( p b ). Thenforall ( p q ) 2 M gradf ( p q )= ( gradf q 1 ( p ), gradf p 2 ( q ) ) Proof. Fix ( p q ) 2 M andconsideranarbitrarytangentvector =( 1 2 ) 2 T ( p q ) M Chooseaparameterizedpath ( r 1 r 2 )= r :[0,1] M suchthat 1 istangentto r 1 ( t ) at p = r 1 ( t 0 ) and 2 istangentto r 2 ( t ) at q = r 2 ( t 0 ) (andthus istangentto r ( t ) at 134

PAGE 135

( p q )= r ( t 0 ) ).Usinglemma 2 ,wehave ( f r ) 0 ( t 0 )= d dt ( f q 1 r 1 ) j t 0 + d dt ( f p 2 r 2 ) j t 0 Fromthisrelationand( B–2 ),wendthat g ( gradf ( p q ), )= g (1) ( gradf q 1 ( p ), 1 )+ g (2) ( gradf p 2 ( q ), 2 ). (B–7) Let gradf ( p q )=( A B ) 2 T ( p q ) M forsomeunknown A 2 T p M B 2 T q M .Usingthe denitionofggivenin( B–6 ),wecanrewrite( B–7 )as g (1) ( A 1 )+ g (2) ( B 2 )= g (1) ( gradf q 1 ( p ), 1 )+ g (2) ( gradf p 2 ( q ), 2 ). Sincethisequalityholdsforall 1 2 T p M andall 2 2 T q M ,wehave A = gradf q 1 ( p ) B = gradf p 2 ( q ) ,whichcompletestheproof. 2 Asthenumberofmanifoldsincreases,thenotation gradf q 1 ( p ) becomesmore cumbersome.Wewillthereforesimplifythenotationbywrit ing gradf ( p ) (tomean gradf q 1 ( p ) )wheneverthemanifoldthegradientfoundwithrespecttois madeobviousby theargument p and q isclearfromcontext. ThefollowingcorollaryisadirectconsequenceofTheorem B.1 Corollary1. GivenasetofnRiemannianmanifolds f ( M i g i ) g ni =1 ,iftheproductRiemannianmetricontheproductmanifold ( M g ) ,where M = M 1 M n ,isdened as g p ( X Y )= g 1 ( X 1 Y 1 )+ + g n ( X n Y n ) for p =( p 1 ,..., p n ) 2 M and X =( X 1 ,..., X n ), Y 2 T p M = T p 1 M 1 ... T p n M n ,then gradf ( p )= ( gradf ( p 1 ),..., gradf ( p n ) ) Wenextconsiderwhatgeodesicslooklikeontheproductmani fold. 135

PAGE 136

TheoremB.2. If M isaproductRiemannianmanifoldasdenedin ( B–6 ) ,thena geodesicbetween p =( p 1 p 2 ) and q =( q 1 q 2 ) 2 M canbeobtainedbytheproductof geodesicsbetween p 1 and q 1 ,and p 2 and q 2 ,on M 1 and M 2 ,respectively. Proof. Let r pq r p 1 q 1 r p 2 q 2 denotethegeodesicson M M 1 ,and M 2 respectively.By denition, r pq =:( r (1) r (2) ) isthepaththatminimizes r pq =argmin r 2 pq Z 1 0 k r 0 ( t ) k 2 dt where pq denotesthesetofallpathson M from p to q and,for r 2 pg r ( t ) denotesa parameterizationofpath r suchthat r (0)= p and r (1)= q .Usingthedenitionofthe inner-productnormontheproductmanifold,weseethat r pq =argmin ( r (1) r (2) ) 2 pq Z 1 0 ( k r (1) 0 ( t ) k 2 + k r (2) 0 ( t ) k 2 ) dt = argmin r 2 p 1 q 1 Z 1 0 k r 0 ( t ) k 2 dt ,argmin r 2 p 2 q 2 Z 1 0 k r 0 ( t ) k 2 dt =( r p 1 q 1 r p 2 q 2 ). 2 Thatparalleltransporton M isgivenbyparalleltransportoneachindividual M i immediatelyfollows.Thusproving( 3–5 ). B.3ProofofTheorem 3.1 proof. Thecostfunction( 3–3 )isafunctionfrom M to R where M istheproduct manifold M := SO (3) R 3 n where n := j V ( k ) j WedenetheRiemannianmetriconthisproductmanifoldasfo llows.For p = ( R 1 t 1 ,..., R n t n ) 2M and X Y 2 T p M ,where X 2 T p M isexpandedas 136

PAGE 137

( X R 1 X t 1 ,..., X R n X t n ) g p ( X Y ):= X i 2 n g R i ( X R i Y R i )+ < X t i Y t i > ThismeetstheconditionsofCorollary 1 andsofor p =( R 1 t 1 ,..., R n t n ) 2M ,wehave gradf ( p )= ( gradf ( R 1 ), gradf ( t 1 ),..., gradf ( R h ), gradf ( t h ) ) Allthatremainsistodetermine gradf ( R u ) and gradf ( t u ) for u =1,..., n .Usinglinearity ofthegradientoperator,wehave gradf ( R u )= X e 2 E ( k ) gradc e ( R u ), and gradf ( t u )= X e 2 E ( k ) gradc e ( t u ). Firstdene f 2 := 1 2 d 2 ( R Tu R v ^ R uv ) f 3 := 1 2 k ^ t uv R Tu ( t v t u ) k 2 ,sothatfor e e ( u v ) c e = f 2 + f 3 .For h = u or h = v ,wethereforehave gradc e ( R h )= gradf 2 ( R h )+ gradf 3 ( R h ) (B–8) gradc e ( t h )=0+ gradf 3 ( t h ). (B–9) TheRiemanniandistance d ( p q ) isbi-invariant,meaningthatforarbitrary R R 2 SO (3) d ( p q )= d ( Rp R Rq R ) .Usingbi-invariance,weobtain d 2 ( R Tu R v ^ R uv )= d 2 ( R v R u ^ R uv ) ,whichimplies gradf 2 ( R v )= gradf 1 ( R v j q ) where f 1 : SO (3) R isgiven by( B–3 )and q = R u ^ R uv .Applyingformula( B–4 ),weobtain gradf 2 ( R v )= R v log( R Tv R u ^ R uv ). 137

PAGE 138

Theexpressionfor gradf 2 ( R u ) issimilarlyobtained;bythinkingof f 2 asafunctionof R u andkeeping R v ^ R uv xed.Thesegradientsarecompactlyrepresentedas: gradf 2 ( R h )= 8>>>>>><>>>>>>: R h log R Th R v ^ R Tuv if h = u R h log R Th R u ^ R uv if h = v 0 otherwise Thegradientofthefunction f 3 : SO (3) R (thinkingof f 3 onlyasafunctionof R u while t u t v ^ t uv arexed)canbeobtainedusing( B–5 )withstraightfrwardbuttedious calculations;whichturnouttobe gradf 3 ( R u )= ( t v t u ) ^ t Tuv + R u ^ t uv ( t v t u ) T R u .The gradientof f 3 : R 3 R issimply ( @ f 3 ( x ) @ x ) T ,whichis gradf 3 ( t h )= 8>>>>>><>>>>>>: t h + R h ^ t uv t v if h = u t h R u ^ t uv t u if h = v 0 o.w. Theseformulaecompletelyspecifythegradientoftheedgec ost, gradc e ( ) in( B–8 ). Theexpressionsfor gradf ( R u ) and gradf ( t u ) for u =1,..., n thatareprovidedinthe theoremareobtainedsimplybyaddingthecomponentsoftheg radientsthatarederived above. 2 B.4GradientoftheCostFunction ( 5–9 ) forHeterogeneousMeasurements AsintheproofofTheorem 3.1 ,thegradientofthefunction f in( 5–9 )atapoint p =( R 1 t 1 ,..., R n t n ) 2 SO (3) R 3 n isgivenby gradf ( p )= X e 2 E ( k ) gradc e ( p )=:( gradf ( R 1 ),..., gradf ( t n )) (B–10) where gradg e ( p ) isthegradientoftheedgecostfunctionforedge e =( u n ) .Finding thegradientofthecostfunction( 5–9 )thenreducestondingthegradientsoftheedge costs c e (speciedin( 5–10 ))foreachedge e 2 E ( k ) 138

PAGE 139

ItfollowsfromCorollary 1 thatthegradientofthecostfunction c e in( 5–10 )forthe edge e =( u v ) 2 E ( k ) is gradc e ( p )= gradc e ( R 1 ), gradc e ( t 1 ),..., gradc e ( R n ), gradc e ( t n ) For ` ( k )( e )= T (pose),thegradientshavealreadybeencomputedintheprev ious section,whichcanbecompactlyrepresentedas( e ( u v ) ) gradc e ( R h )= 8>>>>>>>>><>>>>>>>>>: 2 R h log( R Th R v ^ R Tuv ) + R Th ( t v t u ) ^ t Tuv ^ t uv ( t v t u ) T R h if h = u 2 R h log( R Th R u ^ R uv ) if h = v 0 o.w. gradc e ( t h )=2 I uv ( h )( t u + R u ^ t uv t j ) where I uv ( h )=1 if h = u 1 if h = v and 0 otherwise.If ` ( k )( e )= R (orientation) or ` ( k )( e )= t (position),wehavethefollowingexpressionsforthegradi entfromthe previoussection.If ` ( k )( e )= R gradc e ( R h )= 8>>>>>><>>>>>>: 2 R h log( R Th R v ^ R Tuv ) if h = u 2 R h log( R Th R u ^ R uv ) if h = v 0 otherwise gradc e ( t h )=0. andif ` ( k )( e )= t ,then gradc e ( R h )= 8>><>>: 2 R h R Th ( t v t u ) ^ t Tuv ^ t uv ( t v t u ) T R h if h = u 0 otherwise gradc e ( t h )=2 I uv ( h )( t u + R u ^ t uv t v ). 139

PAGE 140

If ` ( k )( e )= (bearing)or ` ( k )( e )= (distance),thegradient gradc e ( R h ) canbe computedbyusingformula( B–5 ).Thegradient gradc e ( t h ) isobtainedbydifferentiation. Weobtain,for ` ( k )( e )= (bearing) gradc e ( R h )= 8>><>>: 2 R h R Th ( t v t h )^ Tuv k t u t v k ^ uv k t v t u k ( t v t u ) T R h if h = u 0 otherwise gradc e ( t h )= 4 I uv ( h )[( t v t u ) k t v t u k R u ^ uv ] andif ` ( k )( e )= (dist) gradc e ( R h )=0 gradc e ( t h )= 2 I uv ( h ) ( ^ uv k t v t u k ) k t v t u k ( t v t u ). Thegradient gradf ( p ) cannowbecomputedbyusingtheseformulas. 140

PAGE 141

REFERENCES [1]C.F.Olson,L.H.Matthies,M.Schoppers,andM.W.Maimone ,“Rovernavigation usingstereoego-motion,”RoboticsandAutonomousSystems, vol.43,no.4,pp. 215–229,June2003. [2]H.Badino,“Arobustapproachforego-motionestimationu singamobilestereo platform,”inInternationalWorkshoponComplexMotion,20 04,pp.198–208. [3]M.KlopschitzandD.Schmalstieg,“Automaticreconstruct ionofwide-areaducial markermodels,”inInternationalSymposiumonMixedandAugm entedReality,ser. ISMAR'07,Washington,DC,USA,2007. [4]B.StenningandT.Barfoot,“Pathplanningonanetworkofpa ths,”inAerospace Conference,2011IEEE,march2011,pp.1–12. [5]S.Se,H.-K.Ng,P.Jasiobedzki,andT.-J.Moyung,“Visionba sedmodelingand localizationforplanetaryexplorationrovers,”in55thIn ternationalAstronautical Congress,2004. [6]A.Lambert,P.Furgale,T.Barfoot,andJ.Enright,“Visualod ometryaidedbyaSun sensorandinclinometer,”inAerospaceConference,2011IEEE,m arch2011,pp.1 –14. [7]D.Nist er,O.Naroditsky,andJ.Bergen,“Visualodometryforgroundv ehicle applications,”JournalofFieldRobotics,vol.23,p.2006, 2006. [8]R.Jiang,R.Klette,andS.Wang,“Modelingofunboundedlo ng-rangedriftinvisual odometry,”inFourthPacic-RimSymposiumonImageandVideoT echnology, November2010,pp.121–126. [9]G.DubbelmanandF.Groen,“Biasreductionforstereobase dmotionestimation withapplicationstolargescalevisualodometry,”inInter nationalconferenceon ComputerVisionandPatternRecognition,June2009,pp.1–8. [10]G.Sibley,L.Matthies,andG.Sukhatme,“Biasreductionan dlterconvergence forlongrangestereo,”inRoboticsResearch,ser.SpringerT ractsinAdvanced Robotics,S.Thrun,R.Brooks,andH.Durrant-Whyte,Eds.Spring erBerlin/ Heidelberg,2007,vol.28,pp.285–294. [11]G.Grisetti,S.Grzonka,C.Stachniss,P.Pfaff,andW.Burg ard,“EfcientEstimation ofAccurateMaximumLikelihoodMapsin3D,”Proceedingsofthe 2007IEEE/RSJ InternationalConferenceonIntelligentRobotsandSystems ,pp.3472–3478,2007. [12]S.RoumeliotisandG.Bekey,“Distributedmultirobotlo calization,”IEEE TransactionsonRoboticsandAutomation,vol.18,no.5,pp. 781–795,oct2002. 141

PAGE 142

[13]D.Fox,W.Burgard,H.Kruppa,andS.Thrun,“Aprobabilist icapproachto collaborativemulti-robotlocalization,”Autonomousrob ots,vol.8,no.3,pp. 325–344,2000. [14]A.Howard,M.Matark,andG.Sukhatme,“Localizationform obilerobotteams usingmaximumlikelihoodestimation,”inIEEE/RSJInternation alConferenceon IntelligentRobotsandSystems,Lausanne,Switzerland,Septe mber2002. [15]R.Aragues,L.Carlone,G.Calaore,andC.Sagues,“Multi -agentlocalizationfrom noisyrelativeposemeasurements,”inIEEEInternationalCon ferenceonRobotics andAutomation,Shanghai,China,Mar2011. [16]I.Rekleitis,G.Dudek,andE.Milios,“Multi-robotcoop erativelocalization:astudyof trade-offsbetweenefciencyandaccuracy,”inIEEE/RSJIntern ationalConference onIntelligentRobotsandSystems,EPFL,Switzerland,September 2002. [17]A.Martinelli,F.Pont,andR.Siegwart,“Multi-robotloc alizationusingrelative observations,”inIEEEInternationalConferenceonRobotics andAutomation, Barcelona,Spain,April2005. [18]R.C.SmithandP.Cheeseman,“Ontherepresentationande stimationofspatial uncertainty,”InternationalJournalofRoboticsResearch ,vol.5,no.4,pp.56–68, 1986. [19]S.SuandC.S.G.Lee,“Manipulationandpropagationofun certaintyand vericationofapplicabilityofactionsinassemblytasks, ”IEEETransactionson Systems,Man,andCybernetics,vol.22,no.6,pp.1376–1389, 1982. [20]Y.WangandG.S.Chirikjian,“Errorpropagationontheeu clideangroupwith applicationstomanipulatorkinematics,”IEEETransactions onRobotics,vol.VOL. 22,no.4,pp.1–12,Jul2006. [21]Y.WangandG.Chirikjian,“Nonparametric-secondorde rtheoryoferror propagationonmotiongroups,”InternationalJournalofRo boticsResearch, vol.27,no.11-12,pp.1258–1273,2008. [22]Y.ZhouandG.S.Chirikjian,“Probabilisticmodelsofde ad-reckoningerrorin nonholonomnicmobilerobots,”inICRA,2003,pp.1594–1599. [23]P.Smith,T.Drummond,andK.Roussopoulos,“ComputingMAP trajectoriesby representing,propagatingandcombiningpdfsovergroups, ”inIEEEInternational ConferenceonComputerVision,vol.2,2003,pp.1275–1282. [24]J.Kwon,M.Choi,F.Park,andC.Chun,“Particleltering ontheeuclideangroup: frameworkandapplications,”Robotica,vol.25,pp.725–73 7,2007. [25]K.Ni,D.Steedly,andF.Dellaert,“TectonicSAM:Exact,outof-core,submap-based SLAM,”inIEEEInternationalConferenceonRoboticsandAutomat ion,Roma,Italy, May2007. 142

PAGE 143

[26]L.A.A.AnderssonandJ.Nygards,“C-SAM:Multi-robotSLAMusin gsquare rootinformationsmoothing,”inIEEEInternationalConferen ceonRoboticsand Automation,Pasadena,California,USA,May2008. [27]B.Kim,M.Kaess,L.Fletcher,J.Leonard,A.Bachrach,N.Roy ,andS.Teller, “Multiplerelativeposegraphsforrobustcooperativemapp ing,”inIEEEInternational ConferenceonRoboticsandAutomation,Anchorage,Alaska,Ma y2010. [28]V.Indelman,P.Gurl,E.Rivlin,andH.Rotstein,“Distr ibutedvision-aided cooperativelocalizationandnavigationbasedonthree-vi ewgeometry,”inIEEE AerospaceConference,BigSky,Montana,USA,March2011. [29]R.Kurazume,S.Nagata,andS.Hirose,“Cooperativepos itioningwithmultiple robots,”inIEEEInternationalConferenceonRoboticsandAut omation,SanDiego, CA,May1994. [30]C.Sanderson,“Adistributedalgorithmforcooperative navigationamongmultiple mobilerobots,”AdvancedRobotics,vol.12,no.4,pp.335–34 9,1998. [31]V.Caglioti,A.Citterio,andA.Fossati,“Cooperative,d istributedlocalization inmulti-robotsystems:aminimum-entropyapproach,”inIEEE Workshopon DistributedIntelligentSystems,CzechRepublic,June2006 [32]R.SharmaandC.Taylor,“Cooperativenavigationofmavs ingpsdeniedareas,”in IEEEInternationalConferenceonMultisensorFusionandInte grationforIntelligent Systems,Seoul,Korea,Aug.2008. [33]N.Karam,F.Chausse,R.Aufrere,andR.Chapuis,“Locali zationofagroupof communicatingvehiclesbystateexchange,”inIEEE/RSJInterna tionalConference onIntelligentRobotsandSystems,Beijing,China,October20 06. [34]J.KnuthandP.Barooah,“Distributedcollaborativeloca lizationofmultiplevehicles fromrelativeposemeasurements,”in47thAnnualAllertonCon ferenceon Communication,Control,andComputing,Allerton,IL,Octob er2009,pp.314 –321. [35]K.Y.Leung,T.Barfoot,andH.Liu,“Decentralizedlocali zationof sparsely-communicatingrobotnetworks:Acentralized-eq uivalentapproach,” IEEETransactionsonRobotics,vol.26,no.1,pp.62–77,2010. [36]E.Olson,M.Walter,J.Leonard,andS.Teller,“Singleclu stergraphpartitioningfor roboticsapplications,”inProceedingsofRoboticsSciencea ndSystems,2005,pp. 265–272. [37]J.McDonald,M.Kaess,C.Cadena,J.Neira,andJ.Leonard ,“6-DOFmulti-session visualSLAMusinganchornodes,”inEuropeanConferenceonMobi leRobots, ECMR,Orebro,Sweden,Sep2011,pp.69–76. 143

PAGE 144

[38]K.Konolige,J.Bowman,J.D.Chen,P.Mihelich,M.Calonde r,V.Lepetit,and P.Fua,“View-basedmaps,”inProceedingsofRobotics:Science andSystems, Seattle,USA,June2009. [39]C.D.CadenaLerma,J.McDonald,J.J.Leonard,andJ.Nei ra,“Placerecognition usingnearandfarvisualinformation,”inWorldCongress,v ol.18,no.1,2011,pp. 6822–6828. [40]M.CumminsandP.Newman,“Probabilisticlocalizationa ndmappinginthespace ofappearance,”TheInternationalJournalofRoboticsRese arch,vol.27,no.6,pp. 647–665,2008. [41]N.SunderhaufandP.Protzel,“Towardsarobustback-endf orposegraphslam,” inRoboticsandAutomation(ICRA),2012IEEEInternationalCon ferenceon,May 2012,pp.1254–1261. [42]M.Spong,S.Hutchinson,andM.Vidyasagar,RobotModelin gandControl.John Wiley&Sons,Inc.,2006. [43]G.BleserandD.Strickera,“Advancedtrackingthroughef cientimageprocessing andvisual-inertialsensorfusion,”Computers&Graphics, vol.33,no.1,pp.59–72, 2009. [44]B.Williams,M.Cummins,J.Neira,P.Newman,I.Reid,and J.Tard os,“A comparisonofloopclosingtechniquesinmonocularSLAM,”Rob oticsand AutonomousSystems,vol.57,pp.1188–1197,December2009. [45]Y.Ma,S.Soatto,J.Kosecka,andS.S.Sastry,Aninvitation to3-Dvision,ser. Interdisciplinaryappliedmathematics.Springer,2006. [46]K.Kanatani,“Statisticaloptimizationandgeometricinf erenceincomputervision [anddiscussion],”PhilosophicalTransactions:Mathemati cal,Physicaland EngineeringSciences,vol.356,no.1740,pp.1303–1320,May1 998,(withR. Cipolla,R.I.Hartley,W.Triggs,A.Fitzgibbon,O.Faugeras indiscussions). [47]N.I.Fisher,T.Lewis,andB.J.J.Embleton,StatisticalAn alysisofSphericalData. CambridgeUniversityPress,1997. [48]M.FischlerandL.Bolles,“Randomsampleconsensus:Apa radigmformodeltting withapplicationstoimageanalysisandautomatedcartogra phy,”Communications oftheACM,vol.24,pp.381–385,1981. [49]R.HartleyandA.Zisserman,MultipleViewGeometryinCom puterVision. CambridgeUniversityPress,2004. [50]C.TomasiandT.Kanade,“Detectionandtrackingofpoint features,”Carnegie MellonUniversity,Tech.Rep.,April1991. 144

PAGE 145

[51]P.Barooah,W.J.Russell,andJ.P.Hespanha,“Approximat edistributedkalman lteringforcooperativemulti-agentlocalization,”inIn ternationalconferencein DistributedComputinginSensorSystems(DCOSS),SantaBarbara,C A,June 2010. [52]R.TronandR.Vidal,“Distributedimage-based3-dlocal izationofcamerasensor networks,”inIEEEConferenceonDecisionandControl,Shangha i,China, December2009. [53]A.Yershova,S.Jain,S.LaValle,andJ.Mitchell,“Gener atinguniformincremental gridsonSO(3)usingthehopfbration,”TheInternationalJo urnalofRobotics Research,vol.29,no.7,2010. [54]P.Absil,R.Mahony,andR.Sepulchre,OptimizationAlgori thmsonMatrix Manifolds.Princeton,NJ:PrincetonUniversityPress,2008. [55]K.V.MardiaandP.E.Jupp,DirectionalStatistics,ser.Wil eyseriesinprobability andstatistics.Wiley,2000. [56]J.KnuthandP.Barooah,“Errorgrowthinpositionestimati onfromnoisyrelative posemeasurements,”RoboticsandAutonomousSystems,vol.6 1,no.3,pp. 229–244,March2013. [57]R.Kummerle,G.Grisetti,H.Strasdat,K.Konolige,andW. Burgard,“ g 2 0:ageneral frameworkforgraphoptimization,”inIEEEInternationalCon ferenceonRobotics andAutomation,2011,pp.3607–3613. [58]I.V.Melnyk,J.A.Hesch,andS.I.Roumeliotis,“Coopera tiveVision-aidedInertial NavigationUsingOverlappingViews,”IEEEInternationalconf erenceonRobotics andAutomation,pp.1–8,2012. [59]W.G.Breckenridge,“Quaternions-proposedstandardco nventions,”JPL, INTEROFFICEMEMORANDUMIOM343-79-1199,Tech.Rep.,1999. [60]P.T.Fletcher,S.Joshi,C.Lu,andS.Pizer,“GaussianDi stributionsonLieGroups andTheirApplicationtoStatisticalShapeAnalysis,”inInform ationProcessingin MedicalImaging,May2003,pp.450–462. [61]J.Kent,“Someprobabilisticpropertiesofbesselfunct ions,”TheAnnalsof Probability,vol.6,no.5,pp.pp.760–770,1978. [62]S.Gallot,D.Hulin,andJ.LaFontaine,RiemannianGeom etry,3rded.Springer, 2004. [63]D.B.Johnson,“Findingalltheelementarycircuitsofa directedgraph,”SIAM JournalonComputing,vol.4,no.1,pp.77–84,1975. [64]F.E.Grubbs,“Proceduresfordetectingoutlyingobserva tionsinsamples,” Technometrics,vol.11,no.1,pp.1–21,1969. 145

PAGE 146

[65]G.Sibley,G.S.Sukhatme,andL.Matthies,“Constanttime slidingwindow lterslamasabasisformetricvisualperception,”inProc.I EEEInternational ConferenceonRoboticsandAutomationWorkshop,2007,pp.1 0–14. [66]G.Sibley,L.Matthies,andG.Sukhatme,“Aslidingwindow lterforincremental slam,”Unifyingperspectivesincomputationalandrobotvi sion,pp.103–112,2008. [67]S.Lim,T.kyeongLee,S.Lee,S.An,andS.youngOh,“Adapti veslidingwindowfor hierarchicalpose-graph-basedslam,”inControl,Automat ionandSystems(ICCAS), 201212thInternationalConferenceon,Oct.2012,pp.2153– 2158. [68]Y.Ma,J.Ko seck a,andS.Sastry,“Optimizationcriteriaandgeometricalgor ithms formotionandstructureestimation,”InternationalJourn alofComputerVision, vol.44,no.3,pp.219–249,2001. [69]J.KnuthandP.Barooah,“Collaborativelocalizationwit hheterogeneousinter-robot measurementsbyRiemannianoptimization,”inIEEEInternati onalConferenceon RoboticsandAutomation,May2013. [70]K.Grove,H.Karcher,andE.A.Ruh,“Jacobieldsandnslerm etricsoncompact liegroupswithanapplicationtodifferentiablepinchingp roblems.”Mathematische Annalen,vol.211,pp.7–21,1974. 146

PAGE 147

BIOGRAPHICALSKETCH JosephKnuthreceivedhisBachelorofSciencedegreeinCompute rEngineering fromtheUniversityofIllinoisatUrbana-Champaignin2004 .In2008Josephjoinedthe DistributedControlandEstimationLabintheMechanicaland AerospaceEngineering DepartmentattheUniversityofFlorida,Gainesville.Here ceivedaMasterofSciencein 2010andiscurrentlypursuinghisdoctoraldegreeunderthe advisementofDr.Prabir Barooah. 147