Slotine Nonlinear Systems Laboratory Massachusetts Institute of Technology Cambridge Massachusetts 02139 USA yongzhaomitedu jjsmitedu Abstract We derive an exact deterministic nonlinear observer to compute the continuous state of an inertial navigat ID: 24944
Download Pdf The PPT/PDF document "Discrete Nonlinear Observers for Inertia..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
DiscreteNonlinearObserversforInertialNavigationYongZhaoandJean-JacquesE.SlotineNonlinearSystemsLaboratoryMassachusettsInstituteofTechnologyCambridge,Massachusetts,02139,USAyongzhao@mit.edu,jjs@mit.eduWederiveanexactdeterministicnonlinearobservertocomputethecontinuousstateofaninertialnavigationsystembasedonpartialdiscretemeasurements,theso-calledstrap-downproblem.Nonlinearcontractionisusedasthemainanalysistool,andthehierarchicalstructureofthesystemphysicsissystematicallyexploited.Thepaperalsodiscussestheuseofnonlinearmeasurements,suchasdistancestotime-varyingreference1IntroductionThispaperderivesanexactdeterministicnonlinearobservertocomputethecontinuousstateofaninertialnavigationsystembasedonpartialdiscretemeasurements.Themainanalysistoolisnonlinearcontractiontheory[9,10,12,11,14].Recentworkonnonlinearobserverdesignformechanicalsystemsbasedonnonlinearcontractiontheorycanbefoundin[1,3,8,7].SpeciÞcally,weconsidertheclassicalstrap-downproblemininertialnavigation[4,17],whereangularposition(Eulerangles)andinertialpositionarecomputedfromthebodyturnrateandinertialacceleration,measuredcontinuouslyinintrinsic(body-Þxed)coordinates, Asmadeprecisein[12]suchasystemliesattheboundarybetweenconvergenceanddiver-gence,muchlikeatripleintegrator.Inthispaper,thecontinuousmeasurementsofareaugmentedbydiscretementsof,leadingtoagloballyexponentiallyconvergentnonlinearobserverdesign.Suchcombinationsofmeasurementsaretypicalininertialnavigation,whetherforvehiclesorrobots(seee.g.[16]forarecentdiscussion).Thehumanvestibularsystemalsofeaturesasimilarstructure,withotolithicorgansmeasuringlinearaccelerationandsemi-circularcanalsestimatingangularvelocitythroughheavilydampedangularaccelerationsignals,aninforma-tionthencombinedwithvisualdataatmuchslowerupdaterate.Afterabriefreviewofcontractiontheory,Section2introducesthebasicobserverdesign.Webuildsimpleobserverstocomputebasedonpartialdiscretemeasurements.InSection3wediscussextensions,suchastheuseofnonlinearmeasurements,andtheeffectsofsystemdisturbanceandmeasurementdisturbance[14].Wealsostudythecasewheretheinertialnavigationsystemisexpressedinquaternionform[4,5,6].Section4presentssimulationresultsona3-dimensionalsystem.BriefconcludingremarksareofferedinSection5.2BasicAlgorithmInthissection,weÞrstbrießyreviewbasicresultsincontractiontheory.Wethenconstructadiscreteobserverforsystem(1),whichconsistsofahierarchyofthreesub-systems,mirroringthehierarchicalnatureofsystemsphysics(1). 2.1ContractionTheoryThebasictheoremofcontractionanalysis[9]canbestatedasTheorem1Considerthedeterministicsystem,whereisasmoothnonlinearfunction.IfthereexistauniformlypositivedeÞnitemetricsuchthattheassociatedgeneralizedJacobian isuniformlynegativedeÞnite,thenallsystemtrajectoriesthenconvergeexponentiallytoasingletrajectory,withconvergencerate,whereisthelargesteigenvalueofthesymmetricpartof.Thesystemissaidtobecontracting.ItcanbeshownconverselythattheexistenceofauniformlypositivedeÞnitemetricwithrespecttowhichthesystemiscontractingisalsoanecessaryconditionforglobalexponentialconvergenceoftrajectories.Inthelineartime-invariantcase,asystemisgloballycontractingifandonlyifitisstrictlystable,withsimplybeinganormalJordanformofthesystemandthecoordinatetransformationtothatform.Furthermore,since 2(ú+ + whereisthesymmetricpartof,alltransformationscorrespondingtothesametothesameeigenvaluesfor,andthereforetothesamecontractionrateConsidernowahybridcase[11],consistingofacontinuoussystemwhichisswitchedtoadiscretesystemeveryforonediscretestep.Letting,inthesamecoordinatesystembethelargesteigenvalueofthesymmetricmatrix,andbethelargesteigenvalueof(thecor-respondingdiscrete-timequantity,where ,see[11]),asufÞcientconditionfortheoverallsystemtobecontractingis ContractiontheoryproofsandthispapermakeextensiveuseofvirtualdisplacementswhicharedifferentialdisplacementsatÞxedtimeborrowedfrommathematicalphysicsandoptimizationtheory.Formally,ifweviewthepositionofthesystemattimeasasmoothfunctionoftheinitialconditionandoftime,,then 2.2ABasicAlgorithmTheobserverisbasedonthepartialmeasurementsofthestateataseriesofinstants,basedonthediscretemeasurement,computewiththeobserver wheretheÞrstequationdescribesacontinuousupdatebetweenmeasurements,andthesecondequationadiscretemeasurementincorporation.Computingvirtualdisplacementsin(3)leadsto Basedon[12],deÞne.ThisimpliesthatFrom(4),wehave stemsfromtheindifferencepropertyofthesystem[12].Notethatthisindifferencepropertycanbeunderstoodintuitivelyfromthephysicalmean-ingofthetransformationusedtodeÞne.Indeed,,whereisthetransfor-mationmatrixfromEuleranglestobody-Þxedcoordinatesandisthetransformationmatrixfrombody-Þxedcoordinatestoinertialcoordinates.Thus,simplydescribesthevirtualsys-temininertialcoordinates.Alsonotethattheindifferencepropertyisactuallyimmediateinaquaternionrepresentation,asweshalldiscussinSection3.4. Fromhybridcontractioncondition(2)inSection2.1,ifuniformly(5),thenbothtendtozeroexponentially.Sotendstoexponentially.,basedonthediscretemeasurementof,computewiththeobserver i ii From(6)andtheÞrststep,weget A x tendsexponentiallytoaconstant,wehave ii Using(7),thisimpliesthat,whichbycontinuityimpliesthattheconstantwhichtendstomustbezero.Wethushave,exponentially,Sincebydesignisaparticularsolutionof(6),thisimpliesthattendstoexponen-tially.,basedonthediscretemeasurement,usetheobserverSinceweknowtendstozeroexponentially,wehave 5 ,i.e.uniformly(9)isthelargesteigenvalueof.Sotendstoexponentially.Extension1:Whenwecompute,weonlyusethediscrete-timemeasurement.Thisallowstobemeasuredatdifferentinstants,withthesamecomputation.Extension2:Themetriccanalsobewrittenorthogonal.SowecansimplyuseExtension3:Assumethatin(3)wereplacethediscreteupdatelawbythemoregeneralcommute.Then Thehybridcontractioncondition(5)becomesisthelargesteigenvalueofNotethatbecausethegeneralizedJacobiansarezeroateachstepofthehierarchy,thehybridcontractionconditionssimplydeÞnetheinwhichthediscretemeasurementincorporationstepsshouldbecontracting.Asweshallseelater,theßexibilityofferedwithinthisconstraintwillallowustotrade-offmodelerrorvsmeasurementerror,similarlyinspirittoastandardKalmanÞlter.3ExtensionsoftheBasicAlgorithmDiscussionsaboutfulldiscretemeasurements,disturbanceeffects,nonlinearmeasurements,andquaternionrepresentationareofferedinthissection.Anobserverbasedonfullmeasure-mentisdescribedinSection3.1.EffectsofsystemdisturbanceandmeasurementdisturbancearediscussedinSection3.2.Section3.3wedevelopamoregeneraldiscreteobserverappli-cabletononlinearmeasurements.UseofquaternionsisstudiedinSection3.4. 3.1ComputationwithFullDiscreteMeasurementAssumethat,andareactuallymeasured,ataseriesofdiscreteinstantsThensteps1and3areunchanged,butwecanreplacestep2(theestimationof)bytheobserverSinceweknowtendstozeroexponentially,wehave A With,wehaveisthelargesteigenvalueof.Sotendstoexponentially.Notethatinsomecasesoneonlyneedstoestimateorientationandvelocity,andthatthediscretemeasurementofmaybeobtainedfromopticalßow,whichcanbecomputation-allyÓexpensiveÓandthusinfrequent.3.2DisturbanceEffectsEffectsofboundedinputsandmeasurementdisturbancescanbequantiÞedandobservergainschosenaccordingly.Considerinputdisturbanceandmeasurementdisturbance,with,leadingtothemodiÞedsystemUsingthebasicrobustnessresultin[9,14],wecanquantifythecorrespondingquadraticontheestimationerror i7 isthelargesteigenvalueofthesymmetricpartof DeÞnetheobjectivefunction( ,where Weknowshouldalsosatisfyasanupperboundof.Therefore,.Finally,weobtaintheminimumof Whendifferentmeasurementsareavailable,theaboveformulascanalsobeusedtoselectapriorithemostinformativemeasurement.Thiscanbethecaseforinstanceforselectingthedirectionofgazeoftheeyesinhoppingrobot[15].ThiscanalsobethecasewhenthemeasurementsareÓexpensiveÓ,forinstancecomputationally.:Thediscussionsabovewillstillworkwhentheboundsofinputdisturbanceandmeasurementdisturbancearetime-varying.IfSimilartotheabove,wehave 3.3NonlinearmeasurementsForthesystem,considertheobserverWehave x gi yiyi ,wehave.UsingEquation(11)yields xand gi yiyi .ThesufÞcientcontractionconditiononhybridsystemscanbewrittenisthelargesteigenvalueofthesymmetricmatrix.Ifcondition(12)issatisÞedbyanappropriatechoiceof,thenwilltendtoexponentially.Aasimpleillustration,considerusingdistancemeasurementsinsteadofdirectcarte-sianpositionmeasurements.Inthe3-dimensionalspace,measurethedistancesfromonetofourtime-varyingreferencepoints,and (13)9 Thediscrete-updatepartofobserver(10)canbebuiltupasbelow, isa3by3time-varyinggainmatrix.Usingequation(13)yieldswheresubscriptreferstothevalueattimeisnon-singular.ThenwecanchooseWithEquation(15),wehaveBychoosing,wecanmakesatisfythefollowingcontractionconditionthatmakestendstozeroexponentially.isthelargesteigenvalueofthesymmetricmatrix.There-willtendtozero,andwilltendtoexponentially.issingular,onehasEquation(18)isequivalentto whichwecanwriteThismeansthatpointsA,B,C,andDareinthesameplane,andthereforethatthegeometrydoesnotcontainenoughinformationtoinferposition.Tocomputevelocity,onecanrewriteobserver(6)as ti{Ki64375Ki264375}(20)where ! ! ! Wethenhave A x i whichisthesameasequation(7).SimilarlytothesecondstepofSection2,thisshowsthattendstoexponentially.Notethatthegeometryproblemofgoingfromdistancestopositionsissolvedbyady-namicsystem,theobserver,ratherthanexplicitlyateachinstant.Ingeneral,onemayalsouselinearmeasurementsatsomeinstantsandnonlinearonesatothers.Notethatifameasurementisdelayed,thealgorithmsworksimilarlybuttheactualinfor-mationisavailableafterthedelay(i.e.themeasurementisincorporatedatsomepasttimeandtheforwardsimulationrunsinstantlytothecurrenttime). Considernow,extendingsection3.2,theeffectofmodelandmeasurementerrors.ForthemodiÞedsystem,withthefollowingnonlinearobserver,modelerrormeasurementerrorWeknowthequadraticboundsontheestimationerror where gj yiyi xi gj yiyi xigi yigi ,andisthelargesteigenvalueofthesymmetricpartof Wecanchoosethemostrelevantdiscreteupdatefunctionwhichwillbestcontributetoimprovingtheestimate(i.e.,tominimize3.4QuaternionRepresentationAngularpositioncanbeexpressedinquaternionform,avoidingrepresentationsingularities[4,5].Quaternionsexpressarotationofangleabouttheunitvector Withthequaternionvector,thisleadsto where 12 Inthisrepresentation,thefactthatthedynamicsofisindifferentisobvious,sinceskew-symmetric.Theobserverscanbederivedasearlier,simplybyreplacing(3)by basedonthediscretemeasurements.Computingvirtualdisplacements andbecausethedynamicsofisindifferent,weonlyneeduniformly(22)isthelargesteigenvalueof.UnderCondition(22),tendstozeroexpo-nentially,andtendstoexponentially.Theothertwostepsareunchanged,withbeingreplacedbyAlltheabovevariationsandextensionscanofcoursebecombined.4SimulationInthissection,wewilldoa3-dimensionalsimulationaboutsystem(1)basedonthediscreteandthenonlineardistancemeasurements,and,asinSectionConsiderSystem(1)inthe3-dimensionalcase.Where 13 Fourtime-varyingreferencepointsarechosenasbelow(allmoveoncirculartrajectories),Observer(3)withisusedtocompute.Observer(20)withgain(21)isusedto.Usingobserver(10,14)andgain(16),wechoose tosatisfyCondition(12),thuswecancompute.Figure1showstendstoexponentially. 2 4 6 2 0 2 4 6 8 xhat v.s. xTime(sec) 1xhat1x2xhat2x3xhat3 2 4 6 2 0 2 4 6 8 vhat v.s. vTime(sec) 1vhat1v2vhat2v3vhat3 2 4 6 2 0 2 4 6 8 10 rhat v.s. rTime(sec) 1 Figure1:Simulationresultofcomputing,andwiththediscretemeasurements5ConcludingRemarksObserverssimilartothosedevelopedinthispapercaninprinciplebeappliedtoothercontin-uousnonlinearsystemsbesidesinertialnavigationsystems,althoughmuchsimpliÞcationwas affordedbyexploitingthehierarchicalstructureofthesystemphysics.Ananimationofthebasicobserverasappliedtoheadstabilization[2]inasimulatedrobothopper[15]canalsobefoundinhttp://web.mit.edu/nsl/www/hopping robot.htmAcknowledgementThispaperbeneÞtedfromstimulatingdiscussionswithDr.AgostinoReferences[1]Aghannan,N.,Rouchon,P.,AnIntrinsicObserverforaClassofLagrangianSystems,I.E.E.E.Trans.Aut.Control[2]Berthoz,A.,TheBrainÕsSenseofMovement,HarvardUniversityPress,Cambridge,Massachusetts[3]Egeland,O.,Kristiansen,E.,andNguyen,T.D.,ObserversforEuler-BernoulliBeamwithHydraulicDrive,I.E.E.E.Conf.Dec.Control[4]Goldstein,H.,ClassicalMechanics,Addison-Wesley[5]Grassia,F.S.,PracticalParameterizationofRotationsUsingtheExponentialMap,TheJournalofGraphicsTools,Vol.3.3(1998).[6]Hestenes,D.,NewFoundationsforClassicalMechanics,KulwerAcademicPublishers,Dordrecht,The[7]Jouffroy,J.,andJ.Opderbecke,UnderwaterVehicleTrajectoryEstimationUsingContractingPDE-basedObservers,AmericanControlConference,Boston,Massachusetts[8]Kristiansen,D.,andEgeland,O.,TimeandSpatialDiscretizationMethodsthatPreservePassivityPropertiesforSystemsDescribedbyPartialDifferantialEquations,InProceedingsofthe2000AmericanControlConference,Chiago,Illinois[9]Lohmiller,W.,andSlotine,J.J.E.,OnContractionAnalysisforNonlinearSystems,Automatica[10]Lohmiller,W.,ContractionAnalysisofNonlinearSystems,Ph.D.Thesis,DepartmentofMechanicalEn-gineering,MIT[11]Lohmiller,W.,andSlotineJ.J.E.,NonlinearProcessControlUsingContractionTheory,A.I.Ch.E.Journal[12]Lohmiller,W.,andSlotineJ.J.E.,ControlSystemDesignforMechanicalSystemsUsingContractionThe-ory,I.E.E.E.Trans.Aut.Control[13]Slotine,J.J.E.,andLi,W.,AppliedNonlinearControl,Prentice-Hall[14]Slotine,J.J.E.,andLohmiller,W.,Modularity,Evolution,andtheBindingProblem:AViewfromStabilityTheory,NeuralNetworks[15]Raibert,M.H.,LeggedRobotsThatBalance,TheM.I.T.Press,Cambridge,Massachusetts[16]Ryu,J.,RossetterE.J.,andGerdes,J.C.,VehicleSideslipandRollParameterEstimationUsingGPS.ProceedingsofAVEC2002,Hiroshima,Japan[17]Varshalovich,D.A.,Moskalev,A.N.,andKhersonskii,V.K.,DescriptionofRotationinTermsoftheEulerSingapore:WorldScientiÞc