Understanding-the-Basis-of-the-Kalman-Filter

IEEESIGNALPROCESSINGMAGAZINE[128]SEPTEMBER20121053-5888/12/$31.00©2012IEEERamseyFaragher[lectureNOTES]UnderstandingtheBasisoftheKalmanFilterViaaSimpleandIntuitiveDerivationThisarticleprovidesasimpleandintuitivederivationoftheKalmanfilter,withtheaimofteachingthisusefultooltostudentsfromdisci-plinesthatdonotrequireastrongmathematicalbackground.ThemostcomplicatedlevelofmathematicsrequiredtounderstandthisderivationistheabilitytomultiplytwoGaussianfunctionstogetherandreducetheresulttoacompactform.TheKalmanfilterisover50yearsoldbutisstilloneofthemostimportantandcommondatafusionalgorithmsinusetoday.NamedafterRudolfE.Kálmán,thegreatsuccessoftheKalmanfilterisduetoitssmallcompu-tationalrequirement,elegantrecursiveproperties,anditsstatusastheoptimalestimatorforone-dimensionallinearsystemswithGaussianerrorstatistics[1].TypicalusesoftheKalmanfilterincludesmoothingnoisydataandpro-vidingestimatesofparametersofinter-est.Applicationsincludeglobalpositioningsystemreceivers,phase-lockedloopsinradioequipment,smoothingtheoutputfromlaptoptrackpads,andmanymore.Fromatheoreticalstandpoint,theKalmanfilterisanalgorithmpermittingexactinferenceinalineardynamicalsystem,whichisaBayesianmodelsimi-lartoahiddenMarkovmodelbutwherethestatespaceofthelatentvariablesiscontinuousandwherealllatentandobservedvariableshaveaGaussiandis-tribution(oftenamultivariateGaussiandistribution).TheaimofthislecturenoteistopermitpeoplewhofindthisdescriptionconfusingorterrifyingtounderstandthebasisoftheKalmanfil-terviaasimpleandintuitivederivation.RELEVANCETheKalmanfilter[2](anditsvariantssuchastheextendedKalmanfilter[3]andunscentedKalmanfilter[4])isoneofthemostcelebratedandpopu-lardatafusionalgorithmsinthefieldofinformationprocessing.ThemostfamousearlyuseoftheKalmanfilterwasintheApollonavigationcomputerthattookNeilArmstrongtothemoon,and(mostimportantly)broughthimback.Today,Kalmanfiltersareatworkineverysatellitenavigationdevice,everysmartphone,andmanycom-putergames.TheKalmanfilteristypicallyderivedusingvectoralgebraasaminimummeansquaredestimator[5],anapproachsuitableforstudentsconfidentinmathematicsbutnotonethatiseasytograspforstudentsindisciplinesthatdonotrequirestrongmathematics.TheKalmanfilterisderivedherefromfirstprinciplesconsideringasimplephysicalexampleexploitingakeypropertyoftheGaussiandistribution—specificallythepropertythattheproductoftwoGaussiandistributionsisanotherGaussiandistribution.PREREQUISITESThisarticleisnotdesignedtobeathor-oughtutorialforabrand-newstudenttotheKalmanfilter,intheinterestsofbeingconcise,butinsteadaimstopro-videtutorswithasimplemethodofteachingtheconceptsoftheKalmanfil-tertostudentswhoarenotstrongmathematicians.ThereaderisexpectedtobefamiliarwithvectornotationandterminologyassociatedwithKalmanfil-teringsuchasthestatevectorandcova-riancematrix.ThisarticleisaimedatthosewhoneedtoteachtheKalmanfil-tertoothersinasimpleandintuitivemanner,orforthosewhoalreadyhavesomeexperiencewiththeKalmanfilterbutmaynotfullyunderstanditsfounda-tions.Thisarticleisnotintendedtobeathoroughandstandaloneeducationtoolforthecompletenovice,asthatwouldrequireachapter,ratherthanafewpages,toconvey.PROBLEMSTATEMENTTheKalmanfiltermodelassumesthatthestateofasystematatimetevolvedfromthepriorstateattimet-1accord-ingtotheequationxFxBuwtttttt1=++-,(1)where■xtisthestatevectorcontainingthetermsofinterestforthesystem(e.g.,position,velocity,heading)attimet■utisthevectorcontaininganycontrolinputs(steeringangle,throt-tlesetting,brakingforce)■Ftisthestatetransitionmatrixwhichappliestheeffectofeachsys-temstateparameterattimet-1onthesystemstateattimet(e.g.,thepositionandvelocityattimet-1bothaffectthepositionattimet)■BtisthecontrolinputmatrixwhichappliestheeffectofeachDigitalObjectIdentifier10.1109/MSP.2012.2203621Dateofpublication:20August2012THEKALMANFILTERISOVER50YEARSOLDBUTISSTILLONEOFTHEMOSTIMPORTANTANDCOMMONDATAFUSIONALGORITHMSINUSETODAY.IEEESIGNALPROCESSINGMAGAZINE[129]SEPTEMBER2012controlinputparameterinthevectorutonthestatevector(e.g.,appliestheeffectofthethrottleset-tingonthesystemvelocityandposition)■wtisthevectorcontainingtheprocessnoisetermsforeachparame-terinthestatevector.TheprocessnoiseisassumedtobedrawnfromazeromeanmultivariatenormaldistributionwithcovariancegivenbythecovariancematrixQt.Measurementsofthesystemcanalsobeperformed,accordingtothemodelHxvztttt=+,(2)where■ztisthevectorofmeasurements■Htisthetransformationmatrixthatmapsthestatevectorparame-tersintothemeasurementdomain■vtisthevectorcontainingthemeasurementnoisetermsforeachobservationinthemeasurementvec-tor.Liketheprocessnoise,themea-surementnoiseisassumedtobezeromeanGaussianwhitenoisewithcovarianceRt.Inthederivationthatfollows,wewillconsiderasimpleone-dimensionaltrack-ingproblem,particularlythatofatrainmovingalongarailwayline(seeFigure 1).Wecanthereforeconsidersomeexamplevectorsandmatricesinthisproblem.Thestatevectorxtcontainsthepositionandvelocityofthetrainxxxttt=o;E.Thetraindrivermayapplyabrakingoracceleratinginputtothesystem,whichwewillconsiderhereasafunctionofanappliedforceftandthemassofthetrainm.Such