KNOWLEDGE about the mechanical state of a system is

Transcription

1 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 Orienaion Esimaion by Means of Exended Kalman Filer, Quaernions, and Chars P Bernal-Polo and H Marínez-Barberá Absrac An orienaion esimaion algorihm is presened This algorihm is based on he Exended Kalman Filer, and uses quaernions as he orienaion descripor For he filer updae, we use measuremens from an Inerial Measuremen Uni IMU The IMU consiss in a riaxial angular rae sensor, and an also riaxial acceleromeer Quaernions describing orienaions live in he uni sphere of R 4 Knowing ha his space is a manifold, we can apply some basic conceps regarding hese mahemaical objecs, and an algorihm ha reminds he also called Muliplicaive Exended Kalman Filer arises in a naural way The algorihm is esed in a simulaed experimen, and in a real one Index Terms Exended Kalman filer, quaernions, aiude, pose, orienaion, esimaion, IMU, manifold, chars I INTRODUCTION KNOWLEDGE abou he mechanical sae of a sysem is necessary in many engineering fields The orienaion of he sysem is an imporan par of his mechanical sae Fields like roboics, virual realiy, or vehicle navigaion among ohers, could require knowledge of he orienaion of a sysem for asks like: - Conrolling an Unmanned Vehicle - Knowing he orienaion of a camera in a scenario - Knowing he heading of a vehicle in a navigaion sysem - Transforming measuremens aken in he vehicle reference frame o an exern reference frame The problem presens wo main issues ha need o be addressed: We need o choose an orienaion descripor We need o choose an esimaion approach The orienaion of a sysem is undersood as he roaion ransformaion ha relaes wo reference frames: he one whose orienaion we are ineresed in, and he reference frame of he sysem wih respec o which we wan o express such orienaion I only makes sense o speak abou one orienaion wih respec o anoher sysem Knowing ha an orienaion is a roaion ransformaion, our issue is o choose he mos convenien parameerizaion for his roaion ransformaion The mos used parameerizaions are he Euler angles, and heir analogous, Tai-Bryan angles, roaion vecors, roaion marices, and uni quaernions A fairly complee survey of Pablo Bernal is wih Universiy of Murcia pablobernalpolo@gmailcom Humbero Marínez is wih Universiy of Murcia humbero@umes Faculy of compuer science Applied engineering research group orienaion represenaions is given in [ Uni quaernions have properies ha make hem preferable agains he oher parameerizaions Namely: There are no singulariies we avoid he gimbal lock, ha is presen in Euler angles They describe he orienaion in a coninuous way unlike axis-angle represenaion 3 Moion equaions are linear wih quaernions 4 They are deermined by 4 parameers in conras wih a roaion marix, ha needs 9 parameers Because of hese properies, uni quaernions have been he mos widely used orienaion represenaion since he early 98s [, and we also use hem in his work The orienaion esimaion problem has been addressed using several approaches In [3 i is provided a survey of mehods for orienaion esimaion by far more complee han could be given in his work, and i would no make sense o repea i here The Kalman Filer wih is nonlinear versions is he proagonis Bu here is a major issue: uni quaernion do no live in he Euclidean space, where he Kalman Filer is defined This fac leads o a variey of approaches in he applicaion of his formalism In paricular, he known as Muliplicaive Exended Kalman Filer is he mehod of choice because of is accuracy, is relaive simpliciy, is compuaional efficiency, and for being flexible o incorporae a grea variey of measuremens However, here seem o be some aspecs of i ha are sill no well undersood The objecive of his paper is o explore a new view poin for he Exended Kalman Filer applied o he esimaion of orienaions represened by uni quaernions Is final form is very similar o ha of he Muliplicaive Exended Kalman Filer, bu i ges rid of he probably ricky definiion of he rese operaion, and i arises he inroducion of a new updae called char updae No being ha differen he srucure of hese wo algorihms, i would no be unreasonable o rename his MEKF as Manifold Exended Kalman Filer The algorihm developed here is designed o ake measuremens from an Inerial Measuremen Uni IMU which reurns acceleraion, and angular velociy measuremens Ye, his design is easily modifiable in order o adap i o oher ype of sensors The MEKF has been esed in a simulaed experimen, ogeher wih he known Madgwick algorihm [4 I also has been implemened in a real sysem, and esed wih a commercial IMU The remainder of he paper is organized as follows In Secion II, we inroduce he main properies of uni quaernions In Secion III, we inroduce he basic conceps of manifold heory ha will be used in he algorihm developmen In Secion IV,

2 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 we review he moion equaions and measuremen equaions In Secion V, we presen he developed equaions for he sae predicion In Secion VI, we presen he developed equaions for he measuremen predicion In Secion VII, we presen he developed equaions for he filer updaes Secion VIII displays he experimenal resuls Finally, we expose he conclusions, and we picure he fuure work pahways in Secion IX II QUATERNIONS Quaernions are hypercomplex numbers wih hree differen imaginary unis {i, j, k}, and can be expressed as q q + q i + q j + q 3 k They can also be expressed in a vecorial form as q q q q q q q 3 Quaernion produc is defined by he Hamilon axiom i j k i j k, 3 which produces he muliplicaion rule p p q q p q p q + q p + p q 4 Quaernions describing roaions can be buil wih a uni vecor ha defines he roaion axis, q, and he angle of roaion, θ, hrough cosθ/ q 5 q sinθ/ Having his form, hey saisfy he resricion q + q + q + q 3 6 This means ha quaernions describing orienaions live in he uni sphere of R 4 This space has dimension 3, alhough is elemens are deermined using 4 parameers We will use basic conceps of manifold heory o handle his kind of space III BASICS OF MANIFOLD THEORY When dealing wih he Kalman filer, he disribuion of a random variable, x, is encoded by is mean, x, and is covariance marix, P, defined as [ P E x x x x T 7 This can be done when our random variables are elemens of an Euclidean space Bu when a random variable is an elemen of a manifold our covariance marix definiion could lose sense This is our case, where he random variable q q, does no describe an orienaion Then we need o redefine our covariance marix in a differen way, bu we can no change he form of he definiion of he covariance marix if we wan o use he Kalman filer, because his precise form is used in is derivaion We will solve his problem by defining our covariance marix in a differen space Bu firs we will review some previous definiions: a Definiion Manifold: A n-manifold, M n, is a opological space in which each poin is locally homeomorphic o he euclidean space, R n This is, each poin x M n has a neighborhood N M n for which we can define a homeomorphism f : N B n, wih B n he uni ball of R n b Definiion Char: A char for a opological space, M, is a homeomorphism, ϕ, from an open subse, U M, o an open subse of he Euclidean space, V R n This is, a char is a funcion ϕ : U M V R n, wih ϕ a homeomorphism Tradiionally a char is expressed as he pair U, ϕ A The Se of Chars Assume we know he expeced value of our disribuion of quaernions, q In such case, we can express any uni quaernion as q q δ, 8 wih δ anoher uni quaernion uni quaernions ogeher wih heir muliplicaion rule form a group And hen, we can define he se of chars δ ϕ q q, δ, δ 3 9 δ δ δ The se of chars 9, is used in [5, bu his work does no alk abou chars, and wha we call char updae is no applied In each char, ϕ q, he quaernion q is mapped o he origin As he space deformaion produced in he neighborhood of he origin is small, being he variance small, he disribuion in each char will be similar o he disribuion in he manifold The inverse ransformaions for hese chars are given by ϕ q e q q 4 + eq e q B The Transiion Map a Definiion Transiion map: Given wo chars U α, ϕ α and U β, ϕ β describing a manifold, wih U αβ U α U β, a funcion ϕ αβ : ϕ α U αβ ϕ β U αβ can be defined as ϕ αβ x ϕ β ϕ α x, wih x ϕ α U αβ Having he se of chars defined by 9, and having wo chars cenered in quaernions p and q, and relaed by p q δ, hen our ransiion map akes he form e p ϕ q,p e q δ e q δ δ e q δ + δ e q

3 BERNAL-POLO AND MARTÍNEZ-BARBERÁ : ORIENTATION ESTIMATION BY MEANS OF EXTENDED KALMAN FILTER 3 IV MOTION EQUATIONS AND MEASUREMENT EQUATIONS The sae of he sysem is defined by an orienaion, encoded by a uni quaernion q, and by an angular velociy measured in he reference frame aached o our sysem, given by a vecor ω The uni quaernion defines a roaion ransformaion ha deermines he orienaion of he sysem This ransformaion relaes vecors denoed as v expressed in a reference frame aached o he solid whose sae we wan o describe, wih he same vecors denoed as v expressed in an inerial reference frame in which he graviy vecor is expressed as g,, Thus, our roaion ransformaion will yield Using a roaion marix, And using our uni quaernion, v T R v v R v 3 v q v q 4 where his ime v v, and q is he complex conjugae quaernion, ha being q a uni quaernion, i is also is inverse A Moion Equaions Knowing wha our quaernion means, we can wrie he moion equaions for he random variables ha we use o describe he sae of our sysem: q q ω, 5 ω τ, 6 where τ is he process noise, associaed wih he orque acing on he sysem, and is ineria ensor B Measuremen Equaions This work uses an IMU as informaion source We can wrie he measuremen equaions ha relae he random variables describing he sae of our sysem, wih he random variables describing he measuremens of our sensors as follows: a m R T a g + r a, 7 ω m ω + r ω, 8 where r a is he noise in he acceleromeer measuremen, r ω is he noise in he gyroscope measuremen, and a are nongraviaional acceleraions V STATE PREDICTION In his secion we expose he evoluion equaions used o perform he predicion of he expeced value of he sae, and of is covariance marix A Evoluion of he Expeced Value of he Sae Taking he expeced value in equaions 5 and 6, assuming he random variables q and q ω - τ τ dτ o be independen, and he expeced value of he process noise, q ω τ, o be consan when τ [,, our differenial equaions are ransformed ino oher ones whose soluions are ω - ω q ω, 9 q - q - - q ω cos q - - sin ω - ω - ω - ω - B Evoluion of he Sae Covariance Marix Since we need a covariance marix wih a form like 7, we will define he covariance marix of he sae in he se of chars defined by 9 In paricular, for each filer updae we will have an expeced value for he uni quaernion describing he orienaion, q, and a covariance marix defined in he Euclidean space, R 3, whose poins are relaed wih hose of he uni sphere of R 4 hrough he char ϕ q The origin of R 3 is mapped wih he q quaernion, and poins around he origin represen quaernions in he neighborhood of q Differenial Equaions for our Chars: This resul, and is derivaion, is oally inspired and is almos equal o ha which appears in [ Having he definiion for our chars in 8 and 9, we can find he differenial equaions for he δ quaernion using he differenial equaions for q, 5, and for q And having he differenial equaions for he δ quaernion, we can find he differenial equaions for a poin e δ δ on he chars: ė ω [ ω + ω e + e e T ω Noe ha, by he definiion of he chars, he vecor of random variables e is expressed in he char cenered in he quaernion q For each insan,, we have a quaernion q defining he char for ha ime Then he differenial equaion defines he evoluion of he vecor e ha ravels beween chars Differenial Equaions for he Covariance Marix: We define he covariance marix for he sae of our sysem by [ E e e T E [ e ω T P E [ ω e T E [ ω ω T P ee P eω P ωe Noice ha we do no wrie e By he new definiion of he covariance marix, he erm e can be inerpreed as a displacemen from he q quaernion, which is mapped o he origin of R 3 by he char Noe also ha being he covariance marix symmeric, we do no need o find he evoluion of all is erms We jus

4 4 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 need o find he evoluion of he erms P ee, P eω, and We are looking for an esimaion of he covariance marix in, using he informaion in This is, we wan o ge P - from P - - For i is easy o find his relaion Assuming he random variables ω - and q ω o be independens, Q ω 3 If we had a funcion e fe, we could replace in, and perhaps obain a relaion similar o 3 Bu we are no able o find a closed soluion for However, we can find a differenial equaion for P using his differenial equaion for e Saring from, d P Ṗee Ṗ eω d Ṗ ωe Ṗ ωω, 4 wih Ṗ ee E [ ė e T + E [ e ė T, 5 Ṗ eω E [ ė ω T [ + E e ω T 6 Afer replacing, assuming ha higher momens are negligible compared o second-order momens, and remembering our assumpion of independence of he random variables q and q ω, and herefore, of e and τ, 5 and 6 can be approximaed by Ṗ ee P eω T [ ω P ee + + P eω P ee [ ω T 7 Ṗ eω [ ω P eω 8 3 Evoluion Equaions for he Covariance Marix: We are dealing wih a sysem of inhomogeneous linear marix differenial equaions Generally, a sysem of his ype is unreaable, bu in our case he equaions are sufficienly decoupled o be able o find a soluion Given a soluion for, we can find an approximae soluion for P eω And wih his soluion, we can find an approximae soluion for P ee Denoing Ω [ ω we can wrie P eω - e Ω [ P eω P ee - e Ω [ P ee Q ω P eω P eω T - - e ΩT, 9 Q ω + 3 wih Q ω Q ω, being Q ω a consan marix represening he process noise covariance per ime uni VI MEASUREMENT PREDICTION In his secion we expose he measuremen equaions used o perform he predicion of he expeced value of he measuremen, and of is covariance marix A Expeced Value of he Measuremen Expeced Value of he Gyroscope Measuremen: Taking he expeced value on 8, ω m ω + r ω 3 Expeced Value of he Acceleromeer Measuremen: Taking he expeced value on 7, knowing ha he g vecor does no change, and assuming ha he non-graviaional acceleraions affecing our sysem, a, does no depend on is orienaion, a m E [ R T a g + r a 3 Using he uni quaernion describing he orienaion of our sysem, his relaion akes he form a m q a g q + r a 33 And if we use he roaion marix consruced from his uni quaernion, a m R T a g + r a 34 B Covariance Marix of he Measuremen The measuremen is relaed o he sae by means of he measuremen equaions: z hx, r We can approximae linearly he relaionship around he expeced values, x, and r, using a Taylor series z hx, r + H x x + M r r, being H hx, r x x, M r hx, r r, 35 x r he Jacobian marices evaluaed on he expeced value of he random variables Then, our predicion equaion of he measuremen covariance marix akes he form S H P H T + M R M T 36 Gyroscope Block: The measuremen equaion for he gyroscope is linear Using 8 and 35 we obain H ω 37

5 BERNAL-POLO AND MARTÍNEZ-BARBERÁ : ORIENTATION ESTIMATION BY MEANS OF EXTENDED KALMAN FILTER 5 Acceleromeer Block: In order of being consisen wih he Kalman filer formulaion, he acceleraion erm, a, should be par of he noise in he measuremen, since if i were no so, i should be par of he sae Then, measuremen noise in our Kalman filer has wo componens: r : he main measuremen noise This noise comes from he sensor a : non-graviaional acceleraions acing on he sysem These acceleraions obsruc he measuremen of he g vecor Recalling ha we express he covariance marix of he sae in R 3, and knowing ha doing q q in he manifold, is equivalen o do e in his space, we will have for he acceleromeer measuremen equaion: a m a m + h ae, a e e + e a a + h ae, a a a a e + r a r a a a Denoing g R R T a g, and afer some calculus, h a T a R, e a a h a e e e a a g R 3 g R g R g R 3 g R g R 3 Measuremen Covariance Marix: Assuming independence of all random variables involved in he measure, our predicion equaion for he measuremen is where [ g R S [ g R P T + R T Q a R + R a +, 38 R ω Q a is he covariance marix of he random variable a R a is he covariance marix ha describes he noise in he acceleromeer measuremen, which is modeled by he random variable r a R ω is he covariance marix describing he noise in he gyroscope measuremen, which is modeled by he random variable r ω VII UPDATE Alhough he original Kalman filer algorihm jus requires he Kalman updae, he fac ha our covariance marix is expressed in a char makes necessary he compuaion of a second updae A Kalman Updae The Kalman updae is performed in he space where he covariance marix is defined This is, we do no perform he Kalman updae in he manifold, bu in he char Given he esimae of he covariance marix of he sae, P -, and he esimae of he measuremen covariance marix, S -, he opimal Kalman gain is compued as K P - H T S - 39 Given he gain, we can updae he sae disribuion in he usual way: x x - + K z z -, 4 P K H P - 4 The new sae disribuion will be expressed in he char cenered on x In his char, e -, bu e B Manifold Updae In order o find he quaernion corresponding o he updaed vecor e expressed in he char, we mus reverse he funcion ϕ q - e making use of he equaion : q q - δe C Char Updae q e e 4 Afer he Kalman updae, he new sae disribuion is expressed in he char cenered on x - We mus updae he covariance marix expressing i in he char cenered on he updaed sae, x, so ha our informaion is expressed as a he beginning of he ieraion In order of achieve his objecive, we mus use he concep of ransiion maps, ha for our chars ake he form of Being non-linear his relaion, we need o find a linear approximaion: e p e q e p e q + e p e q e q e q 43 eqe q Afer differeniaing our ransiion map and evaluaing in e, having idenified he chars ϕ p ϕ q and ϕ q ϕ q -, we find ou e p e q δ δ [ δ eq e G eq e Then, our updae equaions for he chars are 44 P ee q G P ee q - G T, 45 P eω q G P eω q -, 46 q q - 47 VIII EXPERIMENTAL VALIDATION In his secion we presen resuls obained from a simulaed experimen, and a real one

6 6 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 A Simulaed Experimen Our simulaed experimen consiss on he definiion of a pah, he exracion of simulaed measuremens, he processing of his measuremens, and he evaluaion of he algorihm performance Only knowing he real sae, we are able o define some merics o measure he performance of our algorihm Finally, we display a comparison of he algorihm developed in his paper, which will be called Manifold Exended Kalman Filer MEKF, and he currenly popular algorihm developed by Madgwick [4, whose code can be found in [6 Experimen Seup: For esing our algorihm, we can hink in a simple and inuiive simulaion Le us imagine ha we can freely roam he surface of a orus, which is a manifold whose space can be described in R 3 by xθ, φ R + r cos θ cos φ, 48 yθ, φ R + r cos θ sin φ, 49 zθ, φ r sin θ 5 The orus of our simulaion will have R m, and r 5m We can define a pah in he orus using a hird parameer o se he oher wo: θ v θ, 5 φ v φ 5 We will use he pah defined by v φ rad/s and v θ 3 rad/s, and we will ravel he pah around he orus 3 imes This pah can be seen in Figure Fig Pah followed on he orus in our simulaion Reference frames ha define he orienaion of he IMU can be observed Acceleraions occurring in he IMU can be calculaed by differeniaing wice in 48-5 wih respec o he parameer, resuling ẍθ, φ R + r cos θ cos φ v φ + + r sin θ v θ sin φ v φ + r cos θ v θ cos φ, 53 ÿθ, φ R + r cos θ sin φ v φ + r sin θ v θ cos φ v φ + r cos θ v θ sin φ, 54 zθ, φ r sin θ v θ 55 Now, we can define a reference frame for each poin of he pah The axis of he reference frame will have he direcions of he vecors { x θ, x φ, x θ x φ } 56 Afer making he derivaives in 56, and choosing he direcion of he vecors so ha ẑ poins ouward he orus surface, our roaion marix relaing a vecor measured in he IMU reference frame, wih he same vecor measured in he exernal reference frame will be R x ŷ ẑ cos φ sin φ sin φ cos φ sin θ cos φ sin φ cos θ cos φ sin θ sin φ cos φ cos θ sin φ cos θ sin θ cos θ sin θ sin θ cos θ 57 Marix 57 can be expressed as he produc of 3 known roaion marices: R 58 Recognizing hese marices in 58, and using 5, we can find ou he quaernion describing he orienaion of our reference frame: q cosφ/ cosθ/ sinθ/ sinφ/ Having 59 we can obain he q quaernion: where / 59 / q q φ q θ + q φ q θ q, 6 q φ q θ sinφ/ v φ cosφ/ sinθ/ cosθ/ v θ, 6 6 And wih he q quaernion we can use 5 o ge he angular velociy: ω q q 63 Wih all, we can generae a succession of saes q r ω, and r a m for each sae simulae a measuremen ω Afer ha, m aking only he succession of measuremens, we can make a succession of esimaions abou he sae q e ω using he e orienaion algorihms, and hen compare our esimaion wih he known real sae q r ω r Error Definiion: We will evaluae he performance of he algorihm hrough he definiion of wo errors: Firs, defining g r R r T,, T as he graviy vecor measured in he real reference frame aached o our sysem, and g e R e T,, T as he graviy vecor measured in he esimaed reference frame, we define g e g arccos r g e g r g e 64 The e g error in 64 is defined as he angle beween he vecors g r and g e Being he lowes error he beer, his gives us a measure of how well he algorihm esimaes he direcion of a vecor for which we have direcly relaed

7 BERNAL-POLO AND MARTÍNEZ-BARBERÁ : ORIENTATION ESTIMATION BY MEANS OF EXTENDED KALMAN FILTER 7 measuremens Second, we define q r i, and qr f as he iniial and final quaernions describing he real orienaion of he sysem in our simulaion; and q e i, and qe f as he iniial and final quaernions describing he esimaed orienaion of he sysem Then, defining r, and e as he quaernions describing he roaion ransformaions ha relaes he iniials and finals orienaions by q r f qr i r, and q e f qe i e, we define e θ arccos [ r e 65 The e θ error in 65 is defined as he angle of he roaion defined by he δ θ quaernion, which saisfies e r δ θ Being he lowes error he beer, his gives us a measure of how well he algorihm esimaes he whole orienaion, including heading, for which we do no have direcly relaed measuremens This second error definiion seems unnecessarily complicaed We could hink in somehing like e θ arccos q r f q e f, bu if we sar he simulaion wih an unknown orienaion for he algorihm, his definiion would lead o a differen quaernion from he saring one The e θ error would have a bias because of he ignorance of he iniial heading Our e θ error definiion is independen of his iniial heading ignorance 3 Seing he Algorihm Values: a Iniializaion: The simulaion sars wih a known orienaion sae defined by q, /,, / T, 66 ω, 3, T, 67 P 68 b Characerizaion of Process Noise: The following values have been esablished: q ω, Q ω, 69 a, Q a 7 Sill, afer some esing, we found ha he algorihm behaves similarly wih oher configuraions, provided ha hey are no disproporionae A more worked up algorihm would inroduce dynamical values for his variables c Characerizaion of Measuremen Noise: The Kalman filer requires r a, r ω, 7 in order o produce an unbiased esimaion For he covariance marices we will se R a R ω σ, 7 and we will compare how he error behaves as a funcion of he magniude of he noise in he measuremen Tes Sampling ime s σ Ranging in 3, Fixed o 3 Fixed o Ranging in 7, 3 3 Ranging in 3, Fixed o 7 4 Fixed o 3 Ranging in 7, 3 TABLE I TESTS CONDUCTED IN OUR SIMULATION 4 Simulaion Resuls: We will place our simulaion in 4 differen scenarios, whose deails are displayed in able I, and ha have been chosen according o he curren possibiliies Resuls of Tes and are no shown since he errors produced are oo large This suggess ha boh a good processor small sampling ime as a good sensor small variance are required For Tes 3 and 4 he ime evoluion of error measuremens are ploed in Figures - 5 In Figures 3 and 5 we observe how he error becomes smaller as we improve our IMU decreasing σ, while having a good processor small In Figures and 4 we observe how he error becomes smaller as we improve our processor decreasing, while having a good IMU small σ Graviy error Error in graviy vecor esimaion fixed σ 7 g rad s Madgwick 5 5 ime s MKF Fig Evoluion of he e g error Tes 3 3 s In Figure we observe ha he MEKF increases is accuracy in ime, due o i adds informaion abou he sae in each updae The faser i updaes less he faser i reaches convergence On he oher hand he Madgwick algorihm does no adds informaion, which implies ha i can no learn abou he pas, and i does no increases is accuracy in ime We also observe ha afer reaching a cerain no improvemen in accuracy is seen In Figure 3 we can confirm he same observaion made in he paragraph above Bu we also noe ha i seems o be a limi in he algorihms accuracy as a funcion of he sensor noise I could be an ineresing appreciaion because i could mean ha beyond a cerain sensor qualiy, here would no be

8 8 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 Graviy error Error in graviy vecor esimaion fixed 3 s Madgwick MKF 3 5 σ g rad s Orienaion error Error in orienaion esimaion fixed 3 s Madgwick MKF 3 5 σ g rad s ime s ime s Fig 3 Evoluion of he e g error Tes 4 Fig 5 Evoluion of he e θ error Tes 4 Orienaion error Error in orienaion esimaion fixed σ 7 g rad s Madgwick 5 5 ime s MKF Fig 4 Evoluion of he e θ error Tes 3 3 s Tes Bed: The algorihm has been implemened in a real sysem I has been used a board conaining a MPU65 sensor The processing is performed in he ATmega38P chip conained in an Arduino board In his sysem he sampling ime urns ou o be abou 4 s, wha means ha he algorihm runs abou 5 imes per second The sensor variance is approximaely σ a 4 g, 73 σ ω 4 rad/s 74 Figure 6 shows he assembled sysem, consising in he MPU65 sensor, he Arduino UNO, and a MTi sensor of Xsens an appreciable improvemen in he esimaion In Figure 4 we can noice ha he errors ends o increase over ime I is bes appreciaed for he Madgwick algorihm, and for low updae frequencies wih he MEKF Probably i is no appreciaed for higher updae frequencies because we have no waied enough This happens because we have no reference for orienaion in he plane perpendicular o he graviy vecor If we wan o have a complee non-biased esimaion of he orienaion we should add measuremens from addiional sensors as a magneomeer, or a camera In Figure 5 we again see he same behavior noiced in he previous paragraph We also repea our observaion abou he limi of he esimaion accuracy as a funcion of he sensor noise made wo paragraphs above B Real Experimen Our real experimen consiss on he visual inspecion of he reurned informaion by our algorihm, and he one reurned by he algorihm implemened in a commercial IMU Fig 6 Real sysem composed of an Arduino Uno, a MPU65 chip, and a MTi sensor of Xsens Experimen Resuls: We have described a series of movemens wih he sysem The movemens have been carried ou in four phases The dynamics of each phase has been more aggressive han ha of he previous phase We have ried o finish wih he same orienaion wih which he sysem began Boh have been saved he sensors measuremens and he esimaed saes which are reurned by he algorihms In Figures 7-9 hese daa are shown

9 BERNAL-POLO AND MARTÍNEZ-BARBERÁ : ORIENTATION ESTIMATION BY MEANS OF EXTENDED KALMAN FILTER 9 a m x g a m y g a m z g Measured acceleraions ime s MTi Xsens MPU65 ωx rad/s ωy rad/s ωz rad/s Measured and esimaed angular velociy MPU ime s MTi Xsens MPU65 Fig 7 Acceleraion measuremen during he real experimen Fig 9 Esimaed and measured angular velociy during he real experimen q q q q Esimaed quaernions descibing orienaion ime s MTi Xsens MPU65 Fig 8 Esimaed quaernion describing orienaion during he real experimen The real iniial and final orienaions are ried o mach In Figure 7 we can see ha boh sensor acceleraion measuremens are very similar This makes us hink ha he misalignmen beween he wo sensors is small In Figure 8 we noe ha boh sysems reurn a similar esimaion of he orienaion when he dynamics is no oo aggressive However, afer some aggressive moves, he algorihm presened in his paper has a fas convergence We also noe he bias in he heading esimaion of boh algorihms when we look a q quaernion componen The iniial and final orienaion should be he same, bu we have no reference for orienaion in he plane perpendicular o he graviy vecor In Figure 9 we noe ha he measured angular velociy is very similar o he esimaed angular velociy Perhaps we could accep he gyroscope measuremen as he real angular velociy of our sysem Maybe hen we could ge some advanage in processing speed, and herefore greaer accuracy of our algorihm Bu his is lef for fuure research IX CONCLUSIONS AND FUTURE WORK We have successfully used basic conceps of manifold heory for esimaing orienaions using quaernions as descripors A similar algorihm o he known as Muliplicaive Exended Kalman Filer naurally arises in applying hese conceps wihou having o redefine any aspec of he Exended Kalman Filer The orienaion has been esimaed using measuremens from an IMU, bu he basic idea inroduced in his work is applicable o any oher ype of sensor inended o esimae he orienaion We have esed he algorihm in a real experimen and we have compared our esimaion wih he one given by a commercial IMU, finding ha boh orienaion esimaions are similar This ell us ha our algorihm works as expeced We also have esed he algorihm in a simulaion We have compared he performance of our algorihm wih he algorihm developed by Madgwick The resuls sugges ha he algorihm developed in his paper could achieve a beer accuracy han he one achieved by he Madgwick algorihm However we dare no say so, as here may be various sources of error ha we have no considered: - The updae frequency depends on he processor In he ATmega38P chip, conained in an Arduino board, he Madgwick algorihm is 6 imes faser han he MEKF - The chosen pah could lead o pahological behaviors because of is symmery - We have seen he resul of jus a pah These consideraions lead us o he following issues ha will be addressed in fuure work: We will design a simulaion wih resuls based on he averaging of muliple rajecories, and free of pahological behaviors We will es he algorihm for various char definiions We will es is Unscened Kalman Filer version wih he various char definiions We will compare he MEKF wih he MUKF, and wih he Madgwick algorihm

10 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 We will sudy our sighing abou he limi in he algorihm accuracy as a funcion of he sensor noise REFERENCES [ M D Shuser, A survey of aiude represenaions, Navigaion, vol 8, no 9, pp , 993 [ E J Leffers, F L Markley, and M D Shuser, Kalman filering for spacecraf aiude esimaion, Journal of Guidance, Conrol, and Dynamics, vol 5, no 5, pp 47 49, 98 [3 J L Crassidis, F L Markley, and Y Cheng, Survey of nonlinear aiude esimaion mehods, Journal of guidance, conrol, and dynamics, vol 3, no, pp 8, 7 [4 S O Madgwick, A J Harrison, and R Vaidyanahan, Esimaion of imu and marg orienaion using a gradien descen algorihm, in Rehabiliaion Roboics ICORR, IEEE Inernaional Conference on IEEE,, pp 7 [5 F L Markley, Aiude error represenaions for kalman filering, Journal of guidance, conrol, and dynamics, vol 6, no, pp 3 37, 3 [6 S Madgwick 6 Madgwickahrs [Online Available: hps: //gihubcom/arduino-libraries/madgwickahrs APPENDIX A STATE PREDICTION In his appendix we presen in greaer deail he developmens concerning he sae predicion A Evoluion of he Expeced Value of he Sae Evoluion in he Expeced Value of he Angular Velociy: Taking he expeced value in equaion 6, wih q ω ω τ τ τ ω - ω q ω, - τ τ dτ Evoluion in he Expeced Value of he Orienaion: Taking he expeced value in equaion 5, [ d qτ E dτ E [qτ ω τ [qτ E ω qω τ 75 Assuming he random variables qτ and q ω τ o be independen, d qτ dτ qτ ω qτ qω τ qτ ω τ - 76 This differenial equaion has no general closed soluion Bu if we assume ha he expeced value of he process noise, q ω τ, is consan when τ [,, hen we will have he marix differenial equaion wih ˇΩ qτ ˇΩ qτ, ω ω ω 3 ω ω 3 ω ω ω 3 ω ω 3 ω ω This differenial equaion has he soluion q e ˇΩ - q We can express his soluion using he quaernion produc, as q - q - - q ω cos q - - sin ω - ω - ω - ω - 77 This is, orques acing on he sysem does no depend on is orienaion

11 BERNAL-POLO AND MARTÍNEZ-BARBERÁ : ORIENTATION ESTIMATION BY MEANS OF EXTENDED KALMAN FILTER B Evoluion of he Sae Covariance Marix Differenial Equaions for our Char: a Differenial Equaion for δ: Using 8, 5, and 76, q q δ q q δ + q δ q ω q ω δ + q δ Isolaing he δ quaernion, δ q q ω q q ω δ δ [ δ ω ω δ [ δ δ ω ω δ δ ω ω δ δ ω ω ω + ω δ ω δ δ ω ω + ω δ 78 b Differenial Equaions for e on he Char: Using 9 and 78, ė δ δ δ δ δ ω ω + ω e [ + ω e e or in marix form, ė ω [ ω + ω e + e e T ω 79 Differenial Equaions for he Covariance Marix: a Evoluion Equaion for : E [ ω ω T E [ ω ω ω ω T E [ ω - + q ω ω - + q ω T E [ ω - ω T - - [ + E ω - q ω T + Q ω + E [ q ω ω T [ - + E q ω q ω T Assuming he random variables ω - and q ω o be independens, heir covariance is null In such case, - + Q ω 8, b Differenial Equaion for P ee : Replacing in 5 we obain, [ Ṗ ee E ω e T + [ 4 E e e T ω e T + [ ω [ E e e T [ E [ ω e e T + [ + E e ω T + [ 4 E e ω T e e T + [ E e e T [ ω T [ E e e T [ ω T Here we can see he consequences of reaing a nonlinear sysem The evoluion in he covariance marix P ee, which is composed by momens of second order, is affeced by he higher momens of he disribuion To find he evoluion equaions of he covariance marix we would need informaion abou he momens of order 3 and 4 These may depend of momens of order higher han hem Knowing all he momens of a disribuion would mean o know all saisical informaion Wha we can assume and expec is ha higher momens o be negligible compared o second-order momens In ha case we can wrie, Ṗ ee E [ ω e T [ ω [ E e e T + [ [ + E e ω T E e e T [ ω T P eω T [ ω P ee + + P eω P ee [ ω T 8 c Differenial Equaion for P eω : Replacing in 6 we obain, [ Ṗ eω E ω ω T + [ 4 E e e T ω ω T + [ [ ω E e ω T + E [ [ [ ω e ω T + E e τ T Having assumed he independence of q and q ω, hen e and τ, are also independen Wih ha in mind, and neglecing higher order momens, Ṗ eω E [ ω ω T [ ω E [ e ω T [ ω P eω 8 3 Evoluion Equaions for he Covariance Marix: Denoing Ω [ ω our differenial equaions ake he form: - + Q ω, 83 Ṗ eω τ τ Ω P eω τ, 84 Ṗ ee τ P eω τ T Ω P ee τ P eω τ P ee τ Ω T 86

12 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 a Evoluion Equaion for P eω : Firs of all, le us consider he homogeneous equaion of 84, The soluion for 87 is Ṗ eω τ Ω P eω τ 87 P eω τ e Ω τ P eω To find he soluion o he inhomogeneous differenial equaion, we use he variaion of consans mehod: P eω τ e Ω τ Cτ Ṗ eω τ Ω e Ω τ Cτ + e Ω τ Ċτ Ω P eω τ + e Ω τ Ċτ Idenifying he erms in he differenial equaion we obain he following relaion: Knowing ha we have he informaion a, and we wan o updae he informaion a, our equaion becomes [ P eω - e Ω Ω n n+ - - n +! + + n n Ω n n+ n! n + Q ω + P eω - d 9 Finally, knowing ha calculaing infinie sums would ake a lo, we can runcae in he firs erm, and wrie P eω - e Ω [ P eω Q ω b Evoluion Equaion for P ee : In his case we have he homogeneous equaion Ṗ ee τ Ω P ee τ P ee τ Ω T, 93 e Ω τ Ċτ τ whose soluion is Ċτ e Ω τ τ 88 To solve his las differenial equaion, i is necessary o propose a coninuous evoluion equaion for The simples opion is he linear funcion P ee τ e Ω τ P ee e ΩT τ Using he variaion of consans mehod, P ee τ e Ω τ Cτ e ΩT τ τ + Q ω τ, 89 wih Q ω Q ω, being Q ω a consan marix represening he process noise covariance per uni of ime Having defined his coninuous evoluion equaion, 88 is ransformed ino Ċτ e Ω τ [ + Q ω τ Inegraing 9, Cτ Ω n τ n [ + Q ω τ 9 n! n Ω n τ n+ + n +! n Ω n τ n+ + Q ω + C n! n + n The consan C is deermined by he iniial condiions P eω C Wih his in mind, [ P eω τ e Ω τ Ω n τ n+ + n +! n Ω n τ n+ + Q ω + P eω 9 n! n + n Ṗ ee τ Ω P ee τ + + e Ω τ Ċτ e ΩT τ + P ee τ Ω T Idenifying erms, we deduce he relaion Ċτ e Ω τ [ P eω τ + P eω τ T e ΩT τ Afer subsiuing he expression for P eω, inegrae wih respec o ime, and runcaing in he firs erm of he infinie sums, he soluion we wan is given by [ P ee - e Ω P ee P eω P eω T - - e ΩT APPENDIX B MEASUREMENT PREDICTION Q ω + In his appendix we presen in greaer deail he developmens concerning he measuremen predicion

13 BERNAL-POLO AND MARTÍNEZ-BARBERÁ : ORIENTATION ESTIMATION BY MEANS OF EXTENDED KALMAN FILTER 3 A Expeced Value of he Measuremen Expeced Value of he Acceleromeer Measuremen: Taking he expeced value on 7, a m E [ R T a g + r a The g vecor does no change If we also assume ha he acceleraions affecing our sysem, a, does no depend on is orienaion, a m E [ R T a g + r a One migh be emped o ry o find he expeced value of he marix R wrien as a funcion of he q quaernion, bu hen we would run ino he problem of he compuaion of expeced values such as E[q or E[q q These expeced values are defined in he manifold, and are wha we ry o avoid defining covariance marices in he chars Wha we seek is no he expeced value of he roaion marix, bu somehing like he expeced ransformaion Then, using he quaernion describing he orienaion of our sysem, his expression mus be equivalen o a m q a g q + r a B Covariance Marix of he Measuremen Acceleromeer Block: Knowing ha h a e, a q a g q we will have, h a a h a e T R a a e e a a δ q a g q δ R δ T T R a g,, R δ T δ δ e δ er T a g δ Le us noe ha e R 4 3 However, he erm R δ δ is a 4 marix, whose elemens are again 3 3 marices a Firs erm: Given he expression for he roaion marix corresponding o quaernion δ, R δ δ δ 3 δ δ + δ 3 δ δ δ 3 δ δ T δ δ δ 3 δ δ δ 3 δ δ 3 + δ δ, δ δ 3 + δ δ δ δ 3 δ δ δ δ T we can derive and evaluae a δ,,,, resuling R δ T, δ δ R δ T δ δ R δ T δ δ R δ T δ 3 δ b Second erm: For he chars defined by 9, we can recover he quaernion by Then, δ e 4 + e 3/ And evaluaing a e, δ e e e e e 3 4+e +e 3 e e e e 3 e e 4+e +e 3 e e 3 e 3 e e 3 e 4+e +e,, c The marix: Denoing g R R T a g, and compuing he marix producs, h a e e e a a g R 3 g R g R g R 3 g R g R APPENDIX C UPDATE In his appendix we presen in greaer deail he developmens concerning he filer updae A Char Updae Differeniaing our ransiion map, e p δ [ δ + e q δ + δ e q δ e q δ δ e q δ + δ e q δ T Now, if ϕ p ϕ q, and if ϕ q ϕ q -, and we are ineresed in expressing he covariance marix in he char ϕ q, from he covariance marix expressed in he char ϕ q -, where e q e, hen we mus keep in mind ha e p e δ e δ δ e δ + δ e ϕ δ δe δ

14 4 JOURNAL OF PHYSICAL AGENTS, VOL 8, NO, JULY 7 Then replacing in 43, e p e q Finally, knowing ha δ + δ e δ [ δ δ + δ e eq e δ δe 4 + e, we have e p e q δ 4 + e [ δ eq e δ δ [ δ eq e