ekf-excerptmasterthesis

Chapter4StateEstimationNavigationofanunmannedvehicle,alwaysdependsonagoodestimationofthevehiclestates.Espe-ciallyifnoexternalsensorsormarkersareavailable,moreorlesscomplexalgorithmshavetobeused.Forthisproject,theorientationestimationisdonebythewidelyusedKalmanfilter.However,positionestimationisstillanobjectofongoingresearch.TheheredescribedapproachusesthecompanyownsoftwareRECONSTRUCTME.4.1OrientationWithusingEulerangles(Z−Y′−X′′)between−πandπ,itshouldalsobetakencareofpotentialproblemsreferringtothetransitionbetweenmaxima.Forthisproject,ΦandΘdonotneedspecialtreatment,astherobotisnotexpectedtoexceedthemaxima,sincethiswouldmeanthatthethrustvectorispointingtotheground.ForthesamereasonthesingularityatΘ=90◦oftheusedEuleranglerepresentationoftheorientationisirrelevant.However,theΨanglecanandwillmakethismentionedmaximatransition.ThisdemandsspecialtreatmentconsideringthecalculationofeinthePIDcontrollerinFig.3.2b.Tocompensatethisoverflow,asimplealgorithmcanbeused:1if(eΨπ)2Ψ−=2π;3elseif(eΨ−π)4Ψ+=2π;4.1.1KalmanFilterAgoodintroductiontothetopiccanbefoundin[52].Amoredetaileddescriptionoftheconcept,derivationandpropertiesisgivenin[50].TheKalmanfilter,alsoknownaslinearquadraticestimator,isanoptimalestimatorfordiscretelinearsystemoftheformxk+1=Akxk+wk(4.1)withthestatexk∈Rn,andameasurementzk=Hkxk+vk(4.2)withzk∈Rm,ifprocessnoisewkandthemeasurementnoisevkareassumedtobeindependentrandomvariableswithGaussianprobabilitydensityfunctionsandzeromeanvalue.Thenormalprobabilitydistributionsparethenp(w)∼N(0,Q),withQ=diag{σ2w1,σ2w2,...}p(v)∼N(0,R),withR=diag{σ2v1,σ2v2,...}(4.3)withσ2beingthevarianceofthecorrespondingnoisedistribution.Then×nmatixAkin(4.1)describesthetransitionofthesystembetweentimestepkandk+1,intheabsenceofneitherinputnorprocess274StateEstimation4.1.1KalmanFilter28noise.Them×nmatrixHinthemeasurementequation(4.2)relatesthestatexktothemeasurementzk.Thefilter(inonepopularalgebraicform)consistsoftwostages.First,apredictionofthestateˆx−kattimek,usingthesystemdynamicmatrixAk−1andstateˆxk−1in(4.1)fromthelastiterationk−1.Thiswillbereferredtoasaprioriestimation1.Second,acorrectionofthisestimation,usingthemeasurementszk.Usingtheaprioriestimationˆx−kin(4.2),aninnovationvariable(orresidual)ykcanbedefinedasyk=zk−Hkˆx−k.(4.4)Theinnovationvariableisusedtocorrecttheaprioripredictionusingthen×mgainmatrixKkˆxk=ˆx−k+Kyk.(4.5)Thisestimationisreferredtoasaposterioriestimationˆxk.ThegainmatrixKkdescribesablendingfactorbetweenthemodelbasedprediction,andthemeasurement.ToachieveareadabledescriptionoftheoptimalchoiceforKk,theaprioriandaposterioriestimateerrorsaredefinedase−k≡xk−ˆx−kek≡xk−ˆxk(4.6)andthecorrespondingerrorcovariancesareP−k=E(xk−ˆx−k)(xk−ˆx−k)TPk=E(xk−ˆxk)(xk−ˆxk)T.(4.7)ThecoretaskoftheKalmanfilter,istochoosethegainmatrixKksothattheaposterioiriestimateerrorcovariancePkisminimizedwithrespectofthemeansquareerror.Formoredetailsonthatderivationsee[50].OnepopularformoftheresultingKkthatminimizestheaposterioriestimateerrorcovarianceisthesocalledKalmangainKk=P−kHTkHkP−kHTk+Rk
−1.(4.8)Itcanbeseen,thatthecovarianceRkofthemeasurementnoisedefinedin(4.3)andtheapriorierrorcovarianceP−kdefinedin(4.7)havedirectinfluenceontheoptimalgainmatrixKk.Topointouttheextremecases,ifRkiszeromeaningthatthemeasurementisverytrustworthy,thegainmatrixbecomeslimRk→0Kk=H−1k(4.9)andthus(4.5)degeneratestoˆxk=H−1zk.(4.10)Theestimationthenisonlybasedonthemeasurement.Ontheotherhand,iftheerrorcovarianceP−kapproacheszero,meaningthatthemodelbasedestimationisaccurate,thegainmatrixKkdoesnotconsiderthemeasurementatalllimP−k→0Kk=0ˆxk(K=0)=ˆx−k.(4.11)ThesummaryofthewholeKalmanfilteralgorithmisshowninFig.4.1.1notethesuperscript−inˆx−kandlaterinP−k4StateEstimation4.1.2ExtendedKalmanFilter29InitialestimatesforˆxkandPkaprioriprediction(1)Projectthestateaheadˆx−k+1=Akˆxk(2)ProjecttheerrorcovarianceaheadP−k+1=AkPkATk+Qkaposterioricorrection(1)ComputetheKalmangainKk=P−kHTkHkP−kHTk+Rk
−1(2)Updateestimatewithmeasurementzkˆxk=ˆx−k+Kkzk−Hkˆx−k
(3)UpdatetheerrorcovariancePk=(I−KkHk)P−kFigure4.1:DataflowoftheKalmanfilteroperation4.1.2ExtendedKalmanFilterForestimationofanon-linearsystem,severalextendedversionsoftheKalmanfilterexist.Awidelyusedapproachistolinearizethesystemdynamicsineverysteparoundtheaprioriestimationˆxk,andproceedasforalinearsystem.ThisapproachisknownastheEKF.ThefundamentalimperfectionoftheEKF,asalsopointedoutin[52],isthatthedistributionsofthevariousrandomvariablesarenolongernormal,afterundergoingtheirrespectivenon-lineartransformations.Thus,theoptimalityoftheestimationisonlyapproximatedbylinearization.Thestochasticsystemequationsfrom(4.1)and(4.2)arenowgeneralizedtothenon-linearcasexk+1=f(xk,wk)(4.12)againwiththestatevectorxk∈Rnandzk=h(xk,vk)(4.13)withthemeasurementvectorzk∈Rm.ByderivingtheJacobianmatrixofthepartialderivativesoffandhwithrespecttothestatevectorxkandthenoisevectorswkandvkAlin,k=∂f(xk,wk)∂xkTˆx−k(4.14a)Wlin,k=∂f(xk,wk)∂wkTˆx−k(4.14b)Hlin,k=∂h(xk,vk)∂xkTˆx−k(4.14c)Vlin,k=∂h(xk,vk)∂vkTˆx−k,(4.14d)theEKFisimplementedasshowninFig.4.2.Adetailedde