31/1/2017 Quaternions

Keithp.com/blogs/Quaternions
Tr a c k i n g O r i e n t a t i o n w i t h Q u a t e r n i o n s Keithp.com
Ispenttheflightbackfromchinaandtheweekendaddingorientationtracking ServingfromPortland
toAltOS.I'ddoneabitofresearchoverthelastyearorsoworkingoutthe forawhile.
technique,butthere'salwaysabigstepbetweenreadingaboutsomethingand
actuallydoingit.Iknowthereareapileofquaternionarticlesonthenet,butIwantedtowritedown
preciselywhatIdid,mostlyasaremindertomyselfinthefuturewhenIneedtogofixthecode...

QuaternionBasics
QuaternionswereinventedbySirWilliamRowanHamiltonaround1843.Itseemstohavestartedoffasa
purelytheoreticalpieceofmath,extendingcomplexnumbersfromtwodimensionstofourbyintroducing
twomorerootsof1anddefiningthemtofollow:

i=j=k=ijk=1

Usethesenewrootstocreatenumberswithfourrealcomponents,threeofwhicharemultipliedbyour
threeroots:

r+ix+jy+kz

Withabitofalgebra,youcanfigureouthowtoaddandmultiplythesecompositevalues,usingtheabove
definitiontoreduceandcombinetermssothatyouendupwithasetwhichisclosedundertheusual
operations.

Thenweaddafewmoredefinitions,liketheconjugate:

q=(r+ix+jy+kz)
q*=(rixjykz)

Thenorm:

|q|=(qq*)=(r+x+y+z)

'u'isaunitquaternionifitsnormisone:

|u|=1

QuaternionsandRotation
https://keithp.com/blogs/Quaternions/ 1/5
31/1/2017 Quaternions

QuaternionsandRotation
Ok,sowe'vegotacutelittle4dimensionalalgebra.Howdoesthishelpwithourrotationproblem?Let's
figureouthowtorotateapointinspacebyanarbitraryrotation,definedbyanaxisofrotationandan
amountinradians.

First,takeavector,'v',andconstructaquaternion,'q'asfollows:

q=0+ivx+jvy+kvz

Now,takeaunitquaternion'u',whichrepresentsavectorintheaboveformalongtheaxisofrotation,and
arotationamount,,andconstructaquaternion'r'asfollows:

r=cos/2+usin/2

Withapileofalgebra,youcanshowthattherotationof'q'by'r'is:

q=rqr*

Inaddition,ifyouhavetworotations,'s'and'r',thenthecompositerotation,'t',arotationby'r'followed
by's'canbecomputedwith:

q=s(rqr*)s*

=(sr)q(r*s*)

=(sr)q(sr)*

t=sr

q=tqt*

That'sawholelotsimplerthancarryingarounda3x3matrixtodotherotation,whichmakessenseasa
matrixrepresentationofarotationhasabunchofredundantinformation,anditavoidsapileofproblems
ifyoutrytorepresentthemotionasthreeseparateaxialrotationsperformedinsequence.

C o m p u t i n g a n i n i t i a l ro t a t i o n
Ok,sotherocketissittingonthepad,andit'stiltedslightly.Ineedtocomputetheinitialrotation
quaternionbasedontheaccelerometerreadingswhichprovideavector,'g'pointingup.Essentially,Iwant
tocomputetherotationthatwouldtake'g'andmakeitpointstraightdown.Constructavector'v',which
doespointstraightup:

https://keithp.com/blogs/Quaternions/ 2/5
31/1/2017 Quaternions

g=(0,ax,ay,az)/norm(0,ax,ay,az)
v=(0,0,0,1)

Gis'normalized'sothatitisalsoaunitvector.Thecrossproductbetweengandvwillbeavectornormal
toboth,whichistheaxisofrotation.Asbothgandvareunitvectors,thelengthoftheircrossproductwill
besin

a=gv

=usin

Theanglebetweengandvisthedotproductofthetwovectors,dividedbythelengthofboth.Asbothg
andvareunitvectors,theproductoftheirlengthsisone,sowehave

cos=gv

Forourquaternion,weneedcos/2andsin/2whichwecangetfromthehalfangleformulae:

cos/2=((1+cos)/2)
sin/2=((1cos)/2)

Nowweconstructourquaternionbyfactoringoutsinfromthe'a'and:

q=cos/2+usinsin/2/sin

U p d a t i n g t h e ro t a t i o n b a s e d o n g y ro re a d i n g s
Thegyrosensorreportstherateofrotationalongallthreeaxes,tocomputethechangeinrotation,wetake
theinstantaneoussensorvalueandmultiplyitbythetimesincethelastreadinganddividebytwo
(becausewewanthalfanglesforourquaternions).Withthethreehalfangles,(x,y,z),wecancomputea
compositerotationquaternion:

cosxcosycosz+sinxsinysinz+
i(sinxcosycoszcosxsinysinz)+
j(cosxsinycosz+sinxcosysinz)+
k(cosxcosysinzsinxsinycosz)

Nowwecombinethiswiththepreviousrotationtoconstructourcurrentrotation.

Doingthisfaster

https://keithp.com/blogs/Quaternions/ 3/5
31/1/2017 Quaternions

Ifwereadoursensorfastenoughthattheangleswereasmallfractionofaradian,thenwecouldtake
advantageofthisapproximation:

sinxx
cosx1

thatsimplifiestheabovecomputationconsiderably:

1+xyz+i(xyz)+j(y+xz)+k(zxy)

And,asx,y,z1,wecanfurthersimplifybydroppingthequadraticandcubicelementsasinsignificant:

1+ix+jy+kz

Thisworksatour100Hzsamplingratewhentherotationratesaremodest,butquickmotionswill
introduceabunchoferror.Giventhatwe'vegotplentyofCPUforthistask,there'snoreasontousethis
simplermodel.Ifwedidcrankupthesensorrateabunch,wemightreconsider.

C o m p u t i n g t h e C u r re n t O r i e n t a t i o n
Wehavearotationquaternionwhichmapstheflightframebacktothegroundframe.Tocomputethe
anglefromvertical,wesimplytakeavectorinflightframealongthepathofflight(0,0,0,1)androtate
thatbacktothegroundframe:

g=r(0001)r*

Thatwillbeaunitvectoringroundframepointingalongtheaxisoftherocket.ThearccosineoftheZ
elementwillbetheanglefromvertical.

Results
AlloftheabovecodeischeckedintotheAltOSgitrepository

IaddedatestmodetothefirmwarethatjustdumpsoutthecurrentorientationovertheUSBlinkwhich
letsyouplaywithrotatingtheboardtoseehowwellthesystemtracksthecurrentorientation.There'sabit
ofgyrodrift,asyou'dexpect,butoverall,thesystemtracksthecurrentorientationwithinlessthanatenth
ofadegreepersecond.

Evenwithallofthiscomputationadded,thewholeflightsoftwareisconsuminglessthan7%ofthe
STM32LCPUtime.

LasteditedMon28Oct201303:57:27PMPDT

https://keithp.com/blogs/Quaternions/ 4/5
31/1/2017 Quaternions

https://keithp.com/blogs/Quaternions/ 5/5