11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

TKJElectronics
Developmentwithease
Home
Reviews
Arduino
ARM
PIC
FPGA
TKJElectronics
Webshop

Home>Guides,TKJElectronics>ApracticalapproachtoKalmanfilterandhowtoimplementit
Typetexttosearchhere...

ApracticalapproachtoKalmanfilterandhowtoimplementit
September10th,2012KristianSlothLauszusLeaveacommentGotocomments
IhaveforalongtimebeeninterrestedinKalmanfilersandhowtheywork,IalsousedaKalmanfilterformy
Balancingrobot,butIneverexplainedhowitactuallywasimplemented.ActuallyIhadnevertakenthetimetosit
downwithapenandapieceofpaperandtrytodothemathbymyself,soIactuallydidnotknowhowitwas
implemented.
Itturnedouttobeagoodthing,asIactuallydiscoveredamistakeintheoriginalcode,butIwillgetbacktothat
later.
IactuallywroteabouttheKalmanfilterasmymasterassignmentinhighschoolbackinDecember2011.ButI
onlyusedtheKalmanfiltertocalculatethetruevoltageofaDCsignalmodulatedbyknownGaussianwhite
noise.Myassignmentcanbefoundinthefollowingzipfile:
http://www.tkjelectronics.dk/uploads/Kalman_SRP.zip.Itisindanish,butyoucanproperlyusegoogletranslateto
translatesomeofit.Ifyougotanyspecificquestionsregardingtheassignment,thenaskinthecommentsbelow.
Okay,butbacktothesubject.AsIsadIhadnevertakenthetimetositdownanddothemathregardingthe
Kalmanfilterbasedonanaccelerometerandagyroscope.ItwasnotashardasIexpected,butImustconfessthat
Istillhavenotstudiedthedeepertheorybehind,onwhyitactuallyworks.Butforme,andmostpeopleoutthere,I
ammoreinterrestedinimplementingthefilter,thaninthedeepertheorybehindandwhytheequationsworks.
Beforewebeginyoumusthavesomebasicknowledgeaboutmatriceslikemultiplicationofmatricesand
transposingofmatrices.Ifnotthenpleasetakealookatthefollowingwebsites:
http://en.wikipedia.org/wiki/Matrix_multiplication#Matrix_product_.28two_matrices.29
http://www.mathwarehouse.com/algebra/matrix/multiplymatrix.php
http://en.wikipedia.org/wiki/Transpose
http://en.wikipedia.org/wiki/Covariance_matrix
ForthoseofyouwhodonotknowwhataKalmanfilteris,itisanalgorithmwhichusesaseriesofmeasurements
observedovertime,inthiscontextanaccelerometerandagyroscope.Thesemeasurementswillcontainnoisethat
willcontributetotheerrorofthemeasurement.TheKalmanfilterwillthentrytoestimatethestateofthesystem,
basedonthecurrentandpreviousstates,thattendtobemoreprecisethatthanthemeasurementsalone.
Inthiscontexttheproblemisthattheaccelerometerisingeneralverynoisewhenitisusedtomeasurethe
gravitationalaccelerationsincetherobotismovingbackandforth.Theproblemwiththegyroisthatitdriftsover
timejustlikeaspinningwheelgyrowillstarttofalldownwhenitislosingspeed.
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

1/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Inshortyoucansaythatyoucanonlytrustthegyroscopeonashorttermwhileyoucanonlytrustthe
accelerometeronalongterm.
Thereisactuallyaveryeasywaytodealwiththisbyusingacomplimentaryfilter,whichbasiclyjustconsistofa
digitallowpassfilterontheaccelerometeranddigitalhighpassfilteronthegyroscopereadings.Butitisnotas
accurateastheKalmanfilter,butotherpeoplehavesuccesfullybuildbalancingrobotsusingafinetuned
complimentaryfilter.
Moreinformationaboutgyroscopes,accelerometerandcomplimentaryfilterscanbefoundinthispdf.A
comparisonbetweenacomplimentaryfilterandaKalmanfiltercanbefoundinthefollowingblogpost.
TheKalmanfilteroperatesbyproducingastatisticallyoptimalestimateofthesystemstatebaseduponthe
measurement(s).Todothisitwillneedtoknowthenoiseoftheinputtothefiltercalledthemeasurementnoise,
butalsothenoiseofthesystemitselfcalledtheprocessnoise.TodothisthenoisehastobeGaussiandistributed
andhaveameanofzero,luckilyforusmostrandomnoisehavethischaracteristic.
Formoreinformationaboutthetheorybehindthefiltertakealookatthefollowingpages:
http://en.wikipedia.org/wiki/Kalman_filter
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf
Thesystemstate
Thenextofthisarticlemightseemveryconfusingforsome,butIpromiseyouifyougrabapenandapieceof
paperandtrytofollowalongitisnotthathardifyouarereasonableatmath.
Ifyou,likeme,donothaveacalculatororcomputerprogramthatcanworkwithmatrices,thenIrecommendthe
freeonlinecalculatorWolframAlpha.Iuseditforallthecalculationsinthisarticle.
Iwillusethesamenotationasthewikipediaarticle,butIwillliketonotethatwhenthematrixesareconstantsand
doesnotdependonthecurrenttimeyoudonothavetowritethekafterthem.Soforinstance canbesimplified
to .
AlsoIwouldliketowriteasmallexplanationoftheotheraspectsofthenotations.
FirstIwillmakeanoteaboutwhatscalledthepreviousstate:

Whichisthepreviousestimatedstatebasedonthepreviousstateandtheestimatesofthestatesbeforeit.
Thenextistheaprioristate:

Apriorimeanstheestimateofthestatematrixatthecurrenttimekbasedonthepreviousstateofthesystemand
theestimatesofthestatesbeforeit.
Thelastoneiscalledaposterioristate:

Istheestimatedofthestateattimekgivenobservationsuptoandincludingattimek.
Theproblemisthatthesystemstateitselfishiddenandcanonlybeobservedthroughobservation .Thisisalso
calledaHiddenMarkovmodel.
Thismeansthatthestatewillbebaseduponthestateattimekandallthepreviousstates.Thatalsomeansthat
youcannottrusttheestimateofthestatebeforetheKalmanfilterhasstabilizedtakealookatthegraphatthe
frontpageofmyassignment.
Thehatoverthe meansthatistheestimateofthestate.Unlikejustasingle whichmeansthetruestatethe
onewearetryingtoestimate.
Sothenotationforthestateattimekis:
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

2/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Thestateofthesystemattimekifgivenby:

Where isthestatematrixwhichisgivenby:

Asyoucanseetheoutputofthefilterwillbetheangle butalsothebias baseduponthemeasurementsfrom

theaccelerometerandgyroscope.Thebiasistheamountthegyrohasdrifted.Thismeansthatonecangetthetrue
ratebysubtractingthebiasfromthegyromeasurement.
Thenextisthe matrix,whichisthestatetransitionmodelwhichisappliedtotheprevouisstate

Inthiscase isdefinedas:

Iknowthatthe

mightseemconfusing,butitwillmakesenselater(takealookatmycomment).

Thenextisthecontrolinput ,inthiscaseitisthegyroscopemeasurementindegreespersecond(/s)attimek,
thisisalsocalledtherate .Wewillactuallyrewritethestateequationas:

Thenextthingisthe matrix.Whichiscalledthecontrolinputmodel,whichisdefinedas:

Thismakesperfectlysenseasyouwillgettheangle whenyoumultiplytherate bythedeltatime

wecannotcalculatethebiasdirectlybasedontheratewewillsetthebottomofthematrixto0.

andsince

isprocessnoisewhichisGaussiandistributedwithazeromeanandwithcovariance tothetimek:

istheprocessnoisecovariancematrixandinthiscasethecovariancematrixofthestateestimateofthe
accelerometerandbias.Inthiscasewewillconsidertheestimateofthebiasandtheaccelerometertobe
independent,soitsactuallyjustequaltothevarianceoftheestimateoftheaccelerometerandbias.
Thefinalmatrixisdefinedasso:

Asyoucanseethe covariancematrixdependsonthecurrenttimek,sotheaccelerometervariance andthe

varianceofthebias ismultipliedbythedeltatime .
Thismakessenseastheprocessnoisewillbelargeraslongertimeitissincethelastupdateofthestate.For
instancethegyrocouldhavedrifted.
YouwillhavetoknowtheseconstantsfortheKalmanfiltertowork.
Noteifyousetalargervalue,themorenoiseintheestimationofthestate.Soforinstanceiftheestimatedangle
startstodriftyouhavetoincreasethevalueof .Otherwiseiftheestimatetendstobeslowyouaretrustingthe
estimateoftheangletoomuchandshouldtrytodecreasethevalueof tomakeitmoreresponsive.
Themeasurement
Nowwewilltakealookattheobservationormeasurement ofthetruestate .Theobservation isgivenby:
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

3/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Asyoucanseethemeasurement isgivenbythecurrentstate multipliedbythe matrixplusthe

measurementnoise .
iscalledtheobservationmodelandisusedtomapthetruestatespaceintotheobservedspace.Thetruestate
cannotbeobserved.Sincethemeasurementisjustthemeasurementfromtheaccelerometer, isgivenby:

ThenoiseofthemeasurementhavetobeGaussiandistributedaswellwithazeromeanand asthecovariance:

Butas isnotamatrixthemeasurementnoiseisjustequaltothevarianceofthemeasurement,sincethe
covarianceofthesamevariableisequaltothevariance.Seethispageformoreinformation.
Nowwecandefine asso:

MoreinformationaboutcovariancecanbefoundonWikipediaandinmyassignment.
Wewillassumethatthemeasurementnoiseisthesameanddoesnotdependonthetimek:

Notethatifyousetthemeasurementnoisevariance
toohighthefilterwillrespondreallyslowlyasitis
trustingnewmeasurementsless,butifitistoosmallthevaluemightovershootandbenoisysincewetrustthe
accelerometermeasurementstoomuch.
Sotoroundupyouhavetofindthetheprocessnoisevariances and andthemeasurementvarianceofthe
measurementnoise
.Therearemultiplewaystofindthem,butitisoutoftheaspectofthisarticle.
TheKalmanfilterequations
Okaynowtotheequationswewillusetoestimatethetruestateofthesystemattimek .Somecleverguys
cameupwithequationsfoundbelowtoestimatethestateofthesystem.
Theequationscanbewrittenmorecompact,butIprefertohavethemstretchedout,soitiseasiertoimplement
andunderstandthedifferentsteps.
Predict
Inthefirsttwoequationswewilltrytopredictthecurrentstateandtheerrorcovariancematrixattimek.Firstthe
filterwilltrytoestimatethecurrentstatebasedonallthepreviousstatesandthegyromeasurement:

Thatisalsowhyitiscalledacontrolinput,sinceweuseitasanextrainputtoestimatethestateatthecurrenttime
kcalledtheaprioristate
asdescribedinthebeginningofthearticle.
Thenextthingisthatwewilltrytoestimatetheapriorierrorcovariancematrix
covariancematrix
,whichisdefinedas:

basedonthepreviouserror

Thismatrixisusedtoestimatehowmuchwetrustthecurrentvaluesoftheestimatedstate.Thesmallerthemore
wetrustthecurrentestimatedstate.Theprincipleoftheequationaboveisactuallyprettyeasytounderstand,asit
isprettyobviousthattheerrorcovariancewillincreasesincewelastupdatedtheestimateofthestate,therefore
wemultipliedtheerrorcovariancematrixbythestatetransitionmodel andthetransposeofthat andaddthe
currentprocessnoise attimek.
Theerrorcovariancematrix inourcaseisa22matrix:
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

4/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Update
Thefistthingwewilldoistocomputethedifferencebetweenthemeasurement andtheaprioristate
thisisalsocalledtheinnovation:

Theobservationmodel isusedtomaptheaprioristate
intotheobservedspacewhichisthe
measurementfromtheaccelerometer,thereforetheinnovationisnotamatrix

Thenextthingwewilldoiscalculatewhatscalledtheinnovationcovariance:

Whatitdoesisthatittriestopredicthowmuchweshouldtrustthemeasurementbasedontheapriorierror
covariancematrix
andthemeasurementcovariancematrix .Theobservationmodel isusedtomapthe
apriorierrorcovariancematrix
intoobservedspace.
Thebiggerthevalueofthemeasurementnoisethelargerthevalueof ,thismeansthatwedonottrustthe
incomingmeasurementthatmuch.
Inthiscase isnotamatrixandisjustwrittenas:

ThenextstepistocalculatetheKalmangain.TheKalmangainisusedtotoindicatehowmuchwetrustthe
innovationandisdefinedas:

Youcanseethatifwedonottrusttheinnovationthatmuchtheinnovationcovariance willbehighandifwe
trusttheestimateofthestatethentheerrorcovariancematrix willbesmalltheKalmangainwillthereforebe
smallandoppesiteifwetrusttheinnovationbutdoesnottrusttheestimationofthecurrentstate.
Ifyoutakeadeeperlookyoucanseethatthetransposeoftheobservationmodel isusedtomapthestateofthe
errorcovariancematrix intoobservedspace.Wethencomparetheerrorcovariancematrixbymultiplyingwith
theinverseoftheinnovationcovariance .
Thismakesenseaswewillusetheobservationmodel toextractdatafromthestateerrorcovarianceand
comparethatwiththecurrentestimateoftheinnovationcovariance.
Notethatifyoudonotknowthestateatstartupyoucansettheerrorcovariancematrixlikeso:

Where representalargenumber.
FormybalancingrobotIknowthestartingangleandIfindthebiasofthegyroatstartupbycalibrating,soI
assumethatthestatewillbeknownatstartup,soIinitializetheerrorcovariancematrixlikeso:

Takealookatmycalibrationroutineformoreinformation.
InthiscasetheKalmangainisa21matrix:
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

5/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Nowwecanupdatetheaposterioriestimateofthecurrentstate:

Thisisdonebyaddingtheaprioristate
withtheKalmangainmultipliedbytheinnovation .
Rememberthattheinnovation isthedifferencebetweenthemeasurement andtheestimatedprioristate
,sotheinnovationcanbothbepositiveandnegative.

Alittlesimplifiedtheequationcanbeunderstoodaswesimplycorrecttheestimateoftheaprioristate
wascalculatedusingthepreviousstateandthegyromeasurement,withthemeasurementinthiscasethe
accelerometer.

,that

Thelastthingwewilldoisupdatetheaposteriorierrorcovariancematrix:

Where iscalledtheidentitymatrixandisdefinedas:

Whatthefilterisdoingisthatitisbasicallyselfcorrectingtheerrorcovariancematrixbasedonhowmuchwe
correctedtheestimate.Thismakesenseaswecorrectedthestatebasedtheapriorierrorcovariancematrix
butalsotheinnovationcovariance .

Implementingthefilter
InthissectionIwillusetheequationfromabovetoimplementthefilterintoasimplec++codethatcanbeused
forbalancingrobots,quadcoptersandotherapplicationswhereyouneedtocomputetheangle,biasorrate.
Incaseyouwantthecodenexttoyou,itcanbefoundatgithub:https://github.com/TKJElectronics/KalmanFilter.
IwillsimplywritetheequationsatthetopofeachstepandthensimplifythemafterthatIwillwritehowitiscan
bedoneiCandfinallyIwilllinktocalculationsdoneinWolframAlphainthebottomofeachstep,asIusedthem
todothecalculation.
Step1:

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

6/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Asyoucanseetheaprioriestimateoftheangleis
isequaltotheestimateofthepreviousstate
plustheunbiasedratetimesthedeltatime .
Sincewecannotdirectlymeasurethebiastheestimateoftheaprioribiasisjustequaltothepreviousone.
ThiscanbewritteninClikeso:
rate=newRatebias
angle+=dt*rate
NotethatIcalculatetheunbiasedrate,soitcanbebeusedbytheuseraswell.
WolframAlphalinks:
Eq.1.1
Step2:

TheequationsabovecanbewritteninClikeso:
P[0][0]+=dt*(dt*P[1][1]P[0][1]P[1][0]+Q_angle)
P[0][1]=dt*P[1][1]
P[1][0]=dt*P[1][1]
P[1][1]+=Q_gyroBias*dt
NotethatthisisthepartofthecodethattherewasanerrorinintheoriginalcodethatIused.
WolframAlphalinks:
Eq.2.1
Eq.2.2
Eq.2.3
Eq.2.4
Step3:

TheinnovationcanbecalculatedinClikeso:
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

7/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

y=newAngleangle
WolframAlphalinks:
Eq.3.1
Step4:

AgaintheCcodeisprettysimple:
S=P[0][0]+R_measure
WolframAlphalinks:
Eq.4.1
Step5:

Notethatinothercases canbeamatrixandyoucannotjustsimplydivide by .Insteadyouhaveto

calculatetheinverseofthematrix.Seethefollowingpageformoreinformationonhowtodoso.
TheCimplementationlookslikethis:
K[0]=P[0][0]/S
K[1]=P[1][0]/S
WolframAlphalinks:
Eq.5.1
Step6:

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

8/23

11/8/2016

TKJ Electronics A practical approach to Kalman lter and how to implement it

Yetagaintheequationendupprettyshort,andcanbewrittenassoinC:
angle+=K[0]*y
bias+=K[1]*y
Step7:

Rememberthatwedecreasetheerrorcovariancematrixagain,sincetheerroroftheestimateofthestatehasbeen
decreased.
TheCcodelookslikethis:
floatP00_temp=P[0][0]
floatP01_temp=P[0][1]
P[0][0]=K[0]*P00_temp
P[0][1]=K[0]*P01_temp
P[1][0]=K[1]*P00_temp
P[1][1]=K[1]*P01_temp
WolframAlphalinks:
Eq.7.1
Eq.7.2
Eq.7.3
NotethatIhavefoundthatthefollowingvariancesworksperfectlyformostIMUs:
floatQ_angle=0.001
floatQ_gyroBias=0.003
floatR_measure=0.03
Rememberthatitsveryimportanttosetthetargetangleatstartupifyouneedtousetheoutputatstartup.For
moreinformation,seethecalibrationroutineformybalancingrobot.
IncaseyoumissedithereisthelibraryIwrotealibrarythatcanbeusedbyanymicrocontrollerthatsupports
floatingmath.Thesourcecodecanbefoundatgithub:https://github.com/TKJElectronics/KalmanFilter.
IfyoupreferavideoexplanationabouttheKalmanfilter,Irecommendthefollowingvideoseries:
http://www.youtube.com/watch?v=FkCT_LV9Syk.
Notethatyoucannotusethelibraryifyouneedtorepresentsomethinginafull3Dorientations,aseulerangles
sufferfromwhatiscalledGimballockyouwillneedtouseQuaternionstodothat,butthatisawholenother
story.Fornowtakealookatthefollowingpage.
Thisisallforknow,Ihopethatyouwillfindihelpfull,ifyoudoorhaveanyquestionsfellfreetopostacomment
belowitsupportsLaTeXsyntaxaswell,ifyouneedtowriteequations.
Ifyouspotanyerrorspleaseletmeknowaswell.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-lter-and-how-to-implement-it/

9/23