14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

TKJElectronics
Developmentwithease
Home
Reviews
Arduino
ARM
PIC
FPGA
TKJElectronics
Webshop

Home>Guides,TKJElectronics>ApracticalapproachtoKalmanfilterandhowtoimplementit
Typetexttosearchhere...

ApracticalapproachtoKalmanfilterandhowtoimplementit
September10th,2012KristianLauszusLeaveacommentGotocomments
IhaveforalongtimebeeninterrestedinKalmanfilersandhowtheywork,IalsousedaKalmanfilter
formyBalancingrobot,butIneverexplainedhowitactuallywasimplemented.ActuallyIhadnever
takenthetimetositdownwithapenandapieceofpaperandtrytodothemathbymyself,soI
actuallydidnotknowhowitwasimplemented.
Itturnedouttobeagoodthing,asIactuallydiscoveredamistakeintheoriginalcode,butIwillget
backtothatlater.
IactuallywroteabouttheKalmanfilterasmymasterassignmentinhighschoolbackinDecember
2011.ButIonlyusedtheKalmanfiltertocalculatethetruevoltageofaDCsignalmodulatedby
knownGaussianwhitenoise.Myassignmentcanbefoundinthefollowingzipfile:
http://www.tkjelectronics.dk/uploads/Kalman_SRP.zip.Itisindanish,butyoucanproperlyuse
googletranslatetotranslatesomeofit.Ifyougotanyspecificquestionsregardingtheassignment,
thenaskinthecommentsbelow.
Okay,butbacktothesubject.AsIsadIhadnevertakenthetimetositdownanddothemath
regardingtheKalmanfilterbasedonanaccelerometerandagyroscope.ItwasnotashardasI
expected,butImustconfessthatIstillhavenotstudiedthedeepertheorybehind,onwhyitactually
works.Butforme,andmostpeopleoutthere,Iammoreinterrestedinimplementingthefilter,thanin
thedeepertheorybehindandwhytheequationsworks.
Beforewebeginyoumusthavesomebasicknowledgeaboutmatriceslikemultiplicationofmatrices
andtransposingofmatrices.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
http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

1/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

ForthoseofyouwhodonotknowwhataKalmanfilteris,itisanalgorithmwhichusesaseriesof
measurementsobservedovertime,inthiscontextanaccelerometerandagyroscope.These
measurementswillcontainnoisethatwillcontributetotheerrorofthemeasurement.TheKalman
filterwillthentrytoestimatethestateofthesystem,basedonthecurrentandpreviousstates,that
tendtobemoreprecisethatthanthemeasurementsalone.
Inthiscontexttheproblemisthattheaccelerometerisingeneralverynoisewhenitisusedto
measurethegravitationalaccelerationsincetherobotismovingbackandforth.Theproblemwiththe
gyroisthatitdriftsovertimejustlikeaspinningwheelgyrowillstarttofalldownwhenitislosing
speed.
Inshortyoucansaythatyoucanonlytrustthegyroscopeonashorttermwhileyoucanonlytrustthe
accelerometeronalongterm.
Thereisactuallyaveryeasywaytodealwiththisbyusingacomplimentaryfilter,whichbasiclyjust
consistofadigitallowpassfilterontheaccelerometeranddigitalhighpassfilteronthegyroscope
readings.ButitisnotasaccurateastheKalmanfilter,butotherpeoplehavesuccesfullybuild
balancingrobotsusingafinetunedcomplimentaryfilter.
Moreinformationaboutgyroscopes,accelerometerandcomplimentaryfilterscanbefoundinthispdf.
AcomparisonbetweenacomplimentaryfilterandaKalmanfiltercanbefoundinthefollowingblog
post.
TheKalmanfilteroperatesbyproducingastatisticallyoptimalestimateofthesystemstatebased
uponthemeasurement(s).Todothisitwillneedtoknowthenoiseoftheinputtothefiltercalledthe
measurementnoise,butalsothenoiseofthesystemitselfcalledtheprocessnoise.Todothisthe
noisehastobeGaussiandistributedandhaveameanofzero,luckilyforusmostrandomnoisehave
thischaracteristic.
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,butIpromiseyouifyougrabapenanda
pieceofpaperandtrytofollowalongitisnotthathardifyouarereasonableatmath.
Ifyou,likeme,donothaveacalculatororcomputerprogramthatcanworkwithmatrices,thenI
recommendthefreeonlinecalculatorWolframAlpha.Iuseditforallthecalculationsinthisarticle.
Iwillusethesamenotationasthewikipediaarticle,butIwillliketonotethatwhenthematrixesare
constantsanddoesnotdependonthecurrenttimeyoudonothavetowritethekafterthem.Sofor
instance canbesimplifiedto .
AlsoIwouldliketowriteasmallexplanationoftheotheraspectsofthenotations.
FirstIwillmakeanoteaboutwhatscalledthepreviousstate:

Whichisthepreviousestimatedstatebasedonthepreviousstateandtheestimatesofthestatesbefore
it.
Thenextistheaprioristate:

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

2/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

Apriorimeanstheestimateofthestatematrixatthecurrenttimekbasedonthepreviousstateofthe
systemandtheestimatesofthestatesbeforeit.
Thelastoneiscalledaposterioristate:

Istheestimatedofthestateattimekgivenobservationsuptoandincludingattimek.
Theproblemisthatthesystemstateitselfishiddenandcanonlybeobservedthroughobservation .
ThisisalsocalledaHiddenMarkovmodel.
Thismeansthatthestatewillbebaseduponthestateattimekandallthepreviousstates.Thatalso
meansthatyoucannottrusttheestimateofthestatebeforetheKalmanfilterhasstabilizedtakea
lookatthegraphatthefrontpageofmyassignment.
Thehatoverthe meansthatistheestimateofthestate.Unlikejustasingle whichmeansthetrue
statetheonewearetryingtoestimate.
Sothenotationforthestateattimekis:

Thestateofthesystemattimekifgivenby:

Where isthestatematrixwhichisgivenby:

Asyoucanseetheoutputofthefilterwillbetheangle butalsothebias baseduponthe

measurementsfromtheaccelerometerandgyroscope.Thebiasistheamountthegyrohasdrifted.
Thismeansthatonecangetthetrueratebysubtractingthebiasfromthegyromeasurement.
Thenextisthe matrix,whichisthestatetransitionmodelwhichisappliedtotheprevouisstate
.
Inthiscase isdefinedas:

Iknowthatthe

mightseemconfusing,butitwillmakesenselater(takealookatmycomment).

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

Thenextthingisthe matrix.Whichiscalledthecontrolinputmodel,whichisdefinedas:

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

3/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

Thismakesperfectlysenseasyouwillgettheangle whenyoumultiplytherate bythedeltatime

andsincewecannotcalculatethebiasdirectlybasedontheratewewillsetthebottomofthe
matrixto0.
isprocessnoisewhichisGaussiandistributedwithazeromeanandwithcovariance tothetime
k:

istheprocessnoisecovariancematrixandinthiscasethecovariancematrixofthestateestimate
oftheaccelerometerandbias.Inthiscasewewillconsidertheestimateofthebiasandthe
accelerometertobeindependent,soitsactuallyjustequaltothevarianceoftheestimateofthe
accelerometerandbias.
Thefinalmatrixisdefinedasso:

Asyoucanseethe covariancematrixdependsonthecurrenttimek,sotheaccelerometervariance
andthevarianceofthebias ismultipliedbythedeltatime .
Thismakessenseastheprocessnoisewillbelargeraslongertimeitissincethelastupdateofthe
state.Forinstancethegyrocouldhavedrifted.
YouwillhavetoknowtheseconstantsfortheKalmanfiltertowork.
Noteifyousetalargervalue,themorenoiseintheestimationofthestate.Soforinstanceifthe
estimatedanglestartstodriftyouhavetoincreasethevalueof .Otherwiseiftheestimatetendsto
beslowyouaretrustingtheestimateoftheangletoomuchandshouldtrytodecreasethevalueof
tomakeitmoreresponsive.
Themeasurement
Nowwewilltakealookattheobservationormeasurement ofthetruestate .Theobservation
isgivenby:

Asyoucanseethemeasurement isgivenbythecurrentstate multipliedbythe matrixplus

themeasurementnoise .
iscalledtheobservationmodelandisusedtomapthetruestatespaceintotheobservedspace.The
truestatecannotbeobserved.Sincethemeasurementisjustthemeasurementfromthe
accelerometer, isgivenby:

ThenoiseofthemeasurementhavetobeGaussiandistributedaswellwithazeromeanand asthe
covariance:

Butas isnotamatrixthemeasurementnoiseisjustequaltothevarianceofthemeasurement,since
thecovarianceofthesamevariableisequaltothevariance.Seethispageformoreinformation.
Nowwecandefine asso:

MoreinformationaboutcovariancecanbefoundonWikipediaandinmyassignment.
http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

4/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

Wewillassumethatthemeasurementnoiseisthesameanddoesnotdependonthetimek:

Notethatifyousetthemeasurementnoisevariance
toohighthefilterwillrespondreally
slowlyasitistrustingnewmeasurementsless,butifitistoosmallthevaluemightovershootandbe
noisysincewetrusttheaccelerometermeasurementstoomuch.
Sotoroundupyouhavetofindthetheprocessnoisevariances and andthemeasurement
varianceofthemeasurementnoise
.Therearemultiplewaystofindthem,butitisoutofthe
aspectofthisarticle.
TheKalmanfilterequations
Okaynowtotheequationswewillusetoestimatethetruestateofthesystemattimek .Some
cleverguyscameupwithequationsfoundbelowtoestimatethestateofthesystem.
Theequationscanbewrittenmorecompact,butIprefertohavethemstretchedout,soitiseasierto
implementandunderstandthedifferentsteps.
Predict
Inthefirsttwoequationswewilltrytopredictthecurrentstateandtheerrorcovariancematrixat
timek.Firstthefilterwilltrytoestimatethecurrentstatebasedonallthepreviousstatesandthegyro
measurement:

Thatisalsowhyitiscalledacontrolinput,sinceweuseitasanextrainputtoestimatethestateatthe
currenttimekcalledtheaprioristate
asdescribedinthebeginningofthearticle.
Thenextthingisthatwewilltrytoestimatetheapriorierrorcovariancematrix
previouserrorcovariancematrix
,whichisdefinedas:

basedonthe

Thismatrixisusedtoestimatehowmuchwetrustthecurrentvaluesoftheestimatedstate.The
smallerthemorewetrustthecurrentestimatedstate.Theprincipleoftheequationaboveisactually
prettyeasytounderstand,asitisprettyobviousthattheerrorcovariancewillincreasesincewelast
updatedtheestimateofthestate,thereforewemultipliedtheerrorcovariancematrixbythestate
transitionmodel andthetransposeofthat andaddthecurrentprocessnoise attimek.
Theerrorcovariancematrix inourcaseisa22matrix:

Update
Thefistthingwewilldoistocomputethedifferencebetweenthemeasurement andtheapriori
state
,thisisalsocalledtheinnovation:

Theobservationmodel isusedtomaptheaprioristate
intotheobservedspacewhichisthe
measurementfromtheaccelerometer,thereforetheinnovationisnotamatrix

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

5/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

Thenextthingwewilldoiscalculatewhatscalledtheinnovationcovariance:

Whatitdoesisthatittriestopredicthowmuchweshouldtrustthemeasurementbasedontheapriori
errorcovariancematrix
andthemeasurementcovariancematrix .Theobservationmodel
isusedtomaptheapriorierrorcovariancematrix
intoobservedspace.
Thebiggerthevalueofthemeasurementnoisethelargerthevalueof ,thismeansthatwedonot
trusttheincomingmeasurementthatmuch.
Inthiscase isnotamatrixandisjustwrittenas:

ThenextstepistocalculatetheKalmangain.TheKalmangainisusedtotoindicatehowmuchwe
trusttheinnovationandisdefinedas:

Youcanseethatifwedonottrusttheinnovationthatmuchtheinnovationcovariance willbehigh
andifwetrusttheestimateofthestatethentheerrorcovariancematrix willbesmalltheKalman
gainwillthereforebesmallandoppesiteifwetrusttheinnovationbutdoesnottrusttheestimationof
thecurrentstate.
Ifyoutakeadeeperlookyoucanseethatthetransposeoftheobservationmodel isusedtomapthe
stateoftheerrorcovariancematrix intoobservedspace.Wethencomparetheerrorcovariance
matrixbymultiplyingwiththeinverseoftheinnovationcovariance .
Thismakesenseaswewillusetheobservationmodel toextractdatafromthestateerror
covarianceandcomparethatwiththecurrentestimateoftheinnovationcovariance.
Notethatifyoudonotknowthestateatstartupyoucansettheerrorcovariancematrixlikeso:

Where representalargenumber.
FormybalancingrobotIknowthestartingangleandIfindthebiasofthegyroatstartupby
calibrating,soIassumethatthestatewillbeknownatstartup,soIinitializetheerrorcovariance
matrixlikeso:

Takealookatmycalibrationroutineformoreinformation.
InthiscasetheKalmangainisa21matrix:

Nowwecanupdatetheaposterioriestimateofthecurrentstate:

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

6/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

Thisisdonebyaddingtheaprioristate
withtheKalmangainmultipliedbytheinnovation .
Rememberthattheinnovation isthedifferencebetweenthemeasurement andtheestimated
prioristate
,sotheinnovationcanbothbepositiveandnegative.
Alittlesimplifiedtheequationcanbeunderstoodaswesimplycorrecttheestimateoftheapriori
state
,thatwascalculatedusingthepreviousstateandthegyromeasurement,withthe
measurementinthiscasetheaccelerometer.
Thelastthingwewilldoisupdatetheaposteriorierrorcovariancematrix:

Where iscalledtheidentitymatrixandisdefinedas:

Whatthefilterisdoingisthatitisbasicallyselfcorrectingtheerrorcovariancematrixbasedonhow
muchwecorrectedtheestimate.Thismakesenseaswecorrectedthestatebasedtheapriorierror
covariancematrix
,butalsotheinnovationcovariance .
Implementingthefilter
InthissectionIwillusetheequationfromabovetoimplementthefilterintoasimplec++codethat
canbeusedforbalancingrobots,quadcoptersandotherapplicationswhereyouneedtocomputethe
angle,biasorrate.
Incaseyouwantthecodenexttoyou,itcanbefoundatgithub:
https://github.com/TKJElectronics/KalmanFilter.
IwillsimplywritetheequationsatthetopofeachstepandthensimplifythemafterthatIwillwrite
howitiscanbedoneiCandfinallyIwilllinktocalculationsdoneinWolframAlphainthebottom
ofeachstep,asIusedthemtodothecalculation.
Step1:

Asyoucanseetheaprioriestimateoftheangleis

isequaltotheestimateofthepreviousstate

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

7/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

plustheunbiasedratetimesthedeltatime .
Sincewecannotdirectlymeasurethebiastheestimateoftheaprioribiasisjustequaltotheprevious
one.
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:

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

8/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

TheinnovationcanbecalculatedinClikeso:
y=newAngleangle
WolframAlphalinks:
Eq.3.1
Step4:

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

Notethatinothercases canbeamatrixandyoucannotjustsimplydivide by .Insteadyou

havetocalculatetheinverseofthematrix.Seethefollowingpageformoreinformationonhowtodo
so.
TheCimplementationlookslikethis:
K[0]=P[0][0]/S
http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

9/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

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

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

Rememberthatwedecreasetheerrorcovariancematrixagain,sincetheerroroftheestimateofthe
statehasbeendecreased.
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
http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

10/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

NotethatIhavefoundthatthefollowingvariancesworksperfectlyformostIMUs:
floatQ_angle=0.001
floatQ_gyroBias=0.003
floatR_measure=0.03
Rememberthatitsveryimportanttosetthetargetangleatstartupifyouneedtousetheoutputat
startup.Formoreinformation,seethecalibrationroutineformybalancingrobot.
IncaseyoumissedithereisthelibraryIwrotealibrarythatcanbeusedbyanymicrocontrollerthat
supportsfloatingmath.Thesourcecodecanbefoundatgithub:
https://github.com/TKJElectronics/KalmanFilter.
IfyoupreferavideoexplanationabouttheKalmanfilter,Irecommendthefollowingvideoseries:
http://www.youtube.com/watch?v=FkCT_LV9Syk.
Notethatyoucannotusethelibraryifyouneedtorepresentsomethinginafull3Dorientations,as
euleranglessufferfromwhatiscalledGimballockyouwillneedtouseQuaternionstodothat,but
thatisawholenotherstory.Fornowtakealookatthefollowingpage.
Thisisallforknow,Ihopethatyouwillfindihelpfull,ifyoudoorhaveanyquestionsfellfreeto
postacommentbelowitsupportsLaTeXsyntaxaswell,ifyouneedtowriteequations.
Ifyouspotanyerrorspleaseletmeknowaswell.

Categories:Guides,TKJElectronicsTags:
Comments(251)Trackbacks(3)LeaveacommentTrackback
1.
chfakht
May10th,2015at20:36|#1
Reply|Quote
pleasehowcanispecifietheprocessnoisecovariances,imusingaMPU6050andkalman
filtertoreadvaluefromthegyroscopeandaccelerometer
thnaks
Commentpages
Previous14562868
1. September10th,2012at19:47|#1
Belgaumnews|AboutBelgaum|Belgauminformation|Belgaumdistrict|Belgaumcity|
BelgaumHotels|BelgaumPeople|Belgaumtourism|Belgaumentertainment|Belgaum
students|Insidefacebook|Hack|makeuseof|technicalnews||Kalmanfilterk
2. September11th,2012at06:08|#2
Kalmanfilterkeepsyourbotbalanced|CoolInternetProjects
3. September12th,2012at01:10|#3

bot

Name(required)
EMail(willnotbepublished)(required)
http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

11/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

Website

Subscribetocommentsfeed
I'm not a robot
reCAPTCHA
Privacy - Terms

SubmitComment
CortexM4and9DOFfor$10RaspberryPiGPIOcontrol

HyundaiContrata
CadastreseuCurrculoGrtis!
VejaoNmerodeVagasna

RSS

RecentPosts
IoTArduinoVendingMachine
LaunchPadFlightController
HandheldXV11LIDARwithSTM32F429andMATLAB
IoTfeaturedsoccertable
FullsizeDIYBalancingRobot
USBHostShieldMiniinwebshop
STM32Nucleoanewmbedplatform
NewcontributoratTKJElectronicsDiego
ColorobjecttrackingwithSTM32+OV7725
Year2013forThomas

Categories
Arduino
ARM
Balanduino
Bluetooth
Developmentboards
EmbeddedLinux
FPGA
Guides
mini2440
Multirotors
News
OtherMicroprocessors
PIC
RaspberryPi
Reviews
http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

12/13

14/05/2015

TKJElectronicsApracticalapproachtoKalmanfilterandhowtoimplementit

STM32
TKJElectronics
Tools
USB

Blogroll
Circuits@Home
EEVblog
EEWeb
Github
PCBCalculators
SeeedStudio
Twitter
Webshop
Youtube

Meta
Register
Login
EntriesRSS
CommentsRSS
WordPress.org
TopWordPress
Copyright2015TKJElectronics
ThemebyNeoEase.ValidXHTML1.1andCSS3.

http://blog.tkjelectronics.dk/2012/09/apracticalapproachtokalmanfilterandhowtoimplementit/

13/13