An example of data filtering - Freie Universität

1 Providesa gentleintroductionto theKalman lter,a numericalmethod thatcanbe usedforsensorfusionor ,we considertheKalman lterfora thattheKalman lteris simplya linearweightedaverageoftwo ,we show thatthegeneralcasehasa similarstructureandthatthemathematicalfo rmulationis exampleof data filteringTheKalman lteris widelyusedin aeronauticsandengineeringfortwo mainpurposes:forcombiningmeasurements of thesamevariablesbutfromdi erentsensors,andforcombininganinexactfor ecastof a system'sstatewithaninexactmeasurement of lterhasalsoapplicationsin timeseriesof datapointsx1; x2; : : : ; xn, aforecastercom-putesthebestguessforthepo intxn+1. Asmootherlooksback at thedata,andcomputesthebestpossiblexitaki nginto account thepoints beforeandafterxi. A lterprovidesa correctionforxn+1takinginto account allthepointsx1; x2; : : : ; xnandaninexactmeasurement ofxn+ a lteris thefollowing:Assumethatwe have a systemwhoseone-dimensionalstatewe canmeasureatsuccessive themeasurements arex1; x2; : : : ; xn.

2 Ourtaskis to computetheaverage nof n=1nnX1xi:If a newpointxn+1is measured,we canrecompute n, butit is moree cient tousetheoldvalueof nandmake a smallcorrectionusingxn+1. Thecorrectioniseasyto derive, since n+1=1n+ 1n+1X1xi=nn+ 1 1nnX1xi+1nxn+1!andso, n+1canbe writtenas( ) n+1=nn+ 1 +1n+ 1xn+1= +K(xn+1 )whereK= 1=(n+ 1).Kis n+1is aweightedaverageof theoldestimate nandthenewvaluexn+1. We trust nmorethanthesinglevaluexn+1; therefore,theweight of nis largerthantheweightofxn+1. alsoreadas statingthattheaverageis corrected12 RAUL ROJAS usingthedi erenceofxn+1andtheoldvalue n. ThegainKadjustshow canalsorecalculaterecursivelythequadrati cstandarddeviationof thetimeseries(thevariance).Givennpoints, thequadraticstandarddeviation nis givenby: 2n=1nnX1(xi )2:If a newpointxn+1is measured,thenewvarianceis 2n+1=1n+ 1n+1X1(xi n+1)2=1n+ 1n+1X1(xi n K(xn+1 n)2)whereKis thegainfactorde nedabove andwe canbe expandedas follows: 2n+1=1n+ 1"nX1(xi n)2+ 2 KnX1(xi n)(xn+1 n) +nK2(xn+1 n)2+ (1 K)2(xn+1 n)2#Thesecondterminsidethebracketsis zero,becausePn1(xi n) = 0.

3 Therefore,thewholeexpressionreducesto 2n+1=1n+ 1(n 2n+ (xn+1 )2(nK2+ (1 K)2):SincenK2+ (1 K)2=nKthelastexpressionreducesto 2n+1=nn+ 1( 2n+K(xn+1 )2) = (1 K)( 2n+K(xn+1 )2):Thewholeprocesscannow be castedinto as a seriesof stepsto be followediteratively. Giventhe rstnpoints,andourcalculationof nand n, then Whena newpointxn+1is measured,we computethegainfactorK=1=(n+ 1). We computethenewestimationof theaverage n+1= n+K(xn+1 ) We computealsoa provisionalestimateof thenewstandarddeviation 02n= 2n+K(xn+1 )2 Finally, we ndthecorrect n+1usingthecorrection 2n+1= (1 K) 02nThiskindof iterative computationis usedin calculatorsforupdatingtheaverageandstand arddeviationof numbersenteredsequentiallyinto thecalculatorwithouthavingto iterative procedureshowsthegeneral avor of theKalman lter,which is a kindof recursive showedhow to updatea statisticalquantity thatwe aredealingwithtwo di erentinstruments thatprovidea readingforsomequantity of interestx.)

4 We callx1thereadingfromthe rstinstrument know thatthe rstinstrument hasanerrormodelledby a Gaussianwithstan-darddeviation 1. Theerrorof thesecondinstrument is alsonormallydistributedaroundzerowithsta ndarddeviation 2. We wouldlike to combinebothreadingsinto a bothinstruments areequallygood ( 1= 2), we justtake theaverageof the rstinstrument is absolutelysuperior( 1<< 2), we willkeepx1as ourestimate,andviceversaif thesecondinstrument is clearlysuperiorto the any othercasewe wouldlike to forma weightedaverageof bothreadingsto generateanestimateofxwhich we call^ is which is isweightingeach readinginverselyproportionalto itsprecision,thatis,^x=x1 21+x2 221 21+1 22or simplifying^x=x1 22+x2 21 21+ 22 Thisestimationof ^xful estimatecanbe alsorewrittenas^x=x1+K(x2 x1)wherenow thegainK= 21=( 21+ 22).

5 Theupdateequationhasthesamegeneralformas in theexamplein is optimalgivenourstateof fromtheinstruments is a Gaussian,we canwritetheprobability ofxbeingtheright measurement asp1(x) =1p2 1e 12(x x1)2= 21forinstrument 1, andasp2(x) =1p2 2e 12(x x2)2= 22forinstrument the rstcase,x1is themostprobablemeasurement, in thesecond,x2, butallpointsxhave a non-vanishingprobability of beingtherightmeasurement dueto theinstruments' ROJASS incethetwo measurements areindependent we cancombinethembest,bymultiplyingtheirpro bability obtain:p(x) =p1(x)p2(x) =Ce 12(x x1)2= 21 12(x x2)2= 22whereCis a constant obtainedafterthemultiplication(including thenormalizationfactorneededforthenewGau ssian).Theexpressionforp(x) canbe expandedintop(x) =Ce 12((x2 21 2xx1 21+x21 21)+(x2 22 2xx2 22+x22 22))which groupingsometermsreducestop(x) =Ce 12(x2(1 21+1 21) 2x(x1 21+x2 22))+DwhereDis a constant.

6 Theexpressioncanbe rewrittenasp(x) =Ce 12( 21+ 22 21 22(x2 2x(x1 22+x2 21 21+ 22)))+D:Completingthesquarein theexponent we obtain:p(x) =F e 12 21+ 22 21 22 x (x1 22+x2 21) 21+ 22 2whereallconstants in theexponent (alsothosearisingfromcompletingthesquare )andin front of theexponentialfunctionhave beenabsorbedinto ,we seethatthemostprobableresult^xobtainedfr omcombiningthetwo measurements (thatis, thecenterof thedistribution)is^x=(x1 22+x2 21) 21+ 22andthevarianceof thecombinedresultis^ 2= 21 22 21+ 22If we introducea gainfactorK= 21=( 21+ 22) we canrewritethebestestimate^xof thestateas^x=x1+K(x2 x1)andthechangeto thevariance 21as^ = (1 K) 21 Thisis thegeneralformof theclassicalKalman notneedto be a measurement. It canbe a forecastof thesystemstate,witha variance 21, andx2canbe a measurement withtheerrorvariance 22.

7 TheKalman lterwouldin thatcasecombinetheforecastwiththemeasure ment in orderto providethebestpossiblelinearcombinationo f bothasthe - uncorrelated dimensionsWe cangeneralizetheresultobtainedabove to thecaseof multipledimensions,wheneverydimensionis independent of each other,thatis, whenthereis nocor-relationbetweenthemeasurements obtainedforeach thatcasethemeasurementsx1andx2arevectors ,andwe thecaseof uncorrelateddimensions,given measurementsx1andx2( vectorsof dimensionn), thevariancesforeach dimensioncanbe arrangedin thediagonalof two matrices 1and 2, as follows: 1=2664 2110 00 212 000 00 0 21n3775and 2=2664 2210 00 222 000 00 0 22n3775where 21irepresents thevarianceof thei-thcoordinateforthe rstinstrument,and 22ithevarianceof givenby^x= ( 2x1+ 1x2)( 1+ 2) 1 IntroducingthegainK= 1( 1+ 2) 1, we canrewritetheexpressionabove as^x=x1+K(x2 x1)andthenewestimateof allvariancescanbe putin thediagonalof a matrix^ computedas^ = (I K) 1whereIis then nidentity 1and 2arethecovariancematricesforthetwo uncorrelateddimensions,thecovariancematr ixis thatthecovariancematrix formn-dimensionaldatapointsx1; x2; : : : ; xmis de nedas =1n(x1xT1+x2xT2+ +xmxTm) verysimilarto almostidentical,butnow we have to , we assumethatthetwoinstruments thatproducedthemhave a rstmea-surement is centeredatx1withcovariancematrix 1.

8 Thesecondis centeredatx2withcovariancematrix 2. The rstdistributionis denotedbyN(x1; 1), thesecondbyN(x2; 2)6 RAUL ROJASIf thetwo instruments areindependent, thecombineddistributionis theproductof thetwo two normalizedGaussians,afternormal-ization, is onlyhave to addtheexponents of thetwo Gaussiansin orderto ndtheexponent of constantsin theexponent canbe movedoutof theexponent: theycanbe convertedto con-stants in front of theexponentialfunction,which willbe (^x;^ ).Remember thatin a Gaussianwithcenter andcovariancematrix , theexpo-nent of theGaussianis 12(x )T 1(x ).Thesumof theexponents of thetwo Gaussiansis: 12 (x x1)T 11(x x1) + (x x2)T 12(x x2) We willdropthefactor 1=2 in theintermediatestepsandwillrecover it at (withoutconsideringthefactor 1=2) canbe expandedas:xT( 11+ 12)x 2(xT1 11+xT2 12)x+CwhereC is a symmetricmatricesis commutativeandthetwo covariancematrices 1and 2arecommutative, we cantransformtheexpressionabove into:xT( 1+ 2)( 1 2) 1x 2(xT1 2+xT2 1)( 1 2) 1( 1+ 2) 1( 1+ 2)x+CNotethatwe usedthefactthat( 11+ 12) 1 2= 1+ 2.

9 Now we cancompletethesquarein theexponent andrewritethisexpressionas:(x (x1 2+x2 1)( 1+ 2) 1)T( 1+ 2)( 1 2) 1(x (x1 2+x2 1)( 1+ 2) 1)+C0whereC0is a constant absorbingCandany otherconstants expressionby 1=2, theexponent hasthere-quiredformfora Gaussian,andby directinspectionwe seethattheexpectedaverageof thecombinationof measurements is^x= (x1 2+x2 1)( 1+ 2) 1andthenewcovariancematrixis^ = ( 1+ 2) 1( 1 2)Theseexpressionscorrespondto theKalman lterforthecombinationof two seethismoreclearlywe cande netheKalmangainKasK= 1( 1+ 2) 1 Thentheexpressionsabove canbe rewrittenas^x=x1+K(x2 x1)and^ = (I K) 1whereIis theidentity allthealgebraicmanipulationsabovewe usedthefactthattheproductandthesumof symmetricmatricesis symmetric,andthattheinverseof a symmetricmatrixis differentdimensionTheequationsprintedin booksfortheKalman lteraremoregeneralthantheexpressionswe have derived,becausetheyhandlethemoregeneralc asein whichthemeasurement canhave a di erent dimensionthanthesystem' iseasyto derive themoregeneralform,allis neededis to \move" allcalculationstomeasurement thegeneralcasewe have a processgoingthroughstatetransitionsx1; x2.

10 ThepropagatormatrixAallowsusto forecastthestateat timek+ 1 givenourbestpastestimate^xkof thestateat timek:^x k+1=A^xk+ Theforecastis givenby , a variablewithGaussiandistributioncentered at 0. If thecovariancematrixof thesystemstateestimateattimekis calledP, thecovariancematrixof ^x k+1is givenbyP k+1=APkAT+QwhereQis thecovariancematrixof thenoise .Themeasurementzk+1is is linearlyrelatedto thestate,namelyas follows:zk=Hxk+RwhereHis a matrixandRthecovariancematrixof themeasurement :Kk+1=P k+1HT[HP k+1HT+R] 1 Stateupdate:^xk+1= ^x k+1+Kk+1[zk+1 H^x k+1]Covarianceupdate:Pk+1= (I Kk+1H)P k+1 References1. PeterMaybeck,StochasticModels,Estimation dandControl, , ,\UnderstandingtheKalmanFilter",TheAmeri canStatistician, , , GregWelch andGaryBishop,An Introductionto theKalmanFilter, TR-95-041,Departmentof ComputerScience,University of NorthCarolinaat Chapell Hill,updatedin at Berlin,Institutf ur Informatik