Direction Cosine Matrix IMU: Theory

Transcription

1 Diection Cosine Matix IMU: Theoy William Pemelani and Paul Bizad This is the fist of a pai of papes on the theoy and implementation of a diection-cosine-matix (DCM) based inetial measuement unit fo application in model planes and helicoptes. Actually, at this point, it is still a daft, thee is still a lot moe wok to be done. Seveal eviewes, especially Louis LeGand and UFO-man, have made good suggestions on additions and evisions that we should make and pepaed some figues that we have not included yet. We will eventually incopoate thei suggestions, but it may take a long time to get thee. In the meantime, we think thee is an audience who can benefit fom what we have so fa. The motivation fo DCM was to take the next step in stabilization and contol functions fom an inheently stable aicaft with elevato and udde contol, to an aeobatic aicaft with aileons and elevato. One of the authos (Pemelani) built a two axes boad seveal yeas ago, and developed udimentay fimwae to povide stabilization and etun-to-launch (RTL) functions fo a Gentle Lady sailplane. The fimwae woked well enough, and the autho came to ely on the RTL featue, but it neve seemed to wok as well as the autho would like. In paticula, satisfactoy solutions to the following two issues wee neve found: Mixing. It was ecognized that in a banked tun, thee wee two poblems aising fom the bank angle. Fist, the yaw otation of the aicaft aound the tun geneated a nuisance signal in the pitch gyo, because of the banking. Second, in ode to make a level tun, the elevato needed some up deflection. The amount of deflection depends on the bank angle, which could not be diectly measued. Both issues wee opposite sides of the same coin. Acceleation. An acceleomete measues gavity minus acceleation. The acceleation is equal to the total of all of the aeodynamic foces (lift, thust, dag, etc.) on the plane, plus the gavity foce, divided by the mass. Theefoe, the acceleomete measues the negative of the total of all of the aeodynamic foces. The measuement of gavity is what is needed to level the plane but that is not what you get out of an acceleomete duing acceleated motion. Acceleation is a confounding vaiable. In paticula, when the aicaft pitches up o down, fo a shot while it acceleates in such a way that the output of an acceleomete does not change. Thee is a simila effect that the NASA astonauts expeience when they ae in taining planes. A ballistic path can poduce zeo net foces and theefoe fool acceleometes tempoaily. The combination of this issue and the pevious one pevented eally tight DCM 1 Daft: 5/17/2009

2 pitch contol, and this issue pevented the use of pitch stabilization duing a hand launch. It was ealized that pat of the poblem was not having a six degee of feedom inetial measuement unit (IMU), so it was decided to design a new boad. The UAV DevBoad fom SpakFun was the esult. Coincidentally, one of us (Pemelani) decided he wanted to step up to an aicaft with aileons, and found that he just did not have the needed flying skills. He cashed 5 times in one summe, and had to completely eplace his plane 3 times. So, he decided to use his new boad fo stabilization, shown below, attached to his Goldbeg Enduance with Velco. The question was, how best to do that? Woking togethe, we came to the same conclusion of Mahoney [1]. What is needed is a method that fully espects the nonlineaity of the otation goup. Paul and I decided that we should epesent the otation with a diection cosine matix, that we could maintain the elements of the matix using gyo, acceleomete, and GPS infomation, and that we could use the matix fo contol and navigation. At a high level, hee is how DCM woks: 1. The gyos ae used as the pimay souce of oientation infomation. We integate the nonlinea diffeential kinematic equation that elates the time ate of change in the oientation of the aicaft to its otation ate, and its pesent oientation. This is done at a high ate, (40 to 50 Hz) often enough to give the sevos fesh infomation fo each and evey PWM pulse that is sent to the sevos. DCM 2 Daft: 5/17/2009

3 2. Recognizing that numeical eos in the integation will gadually violate the othogonality constaints that the DCM must satisfy, we make egula, small adjustments to the elements of the matix to satisfy the constaints. 3. Recognizing that numeical eos, gyo dift, and gyo offset will gadually accumulate eos in the DCM elements, we use efeence vectos to detect the eos, and a popotional plus integal (PI) negative feedback contolle between the detected eos and the gyo inputs used in step 1, to dissipate the eos faste than they can build up. GPS is used to detect yaw eo, acceleometes ae used to detect pitch and oll. The pocess is shown schematically in Figue 1. [XYZGyos] Gyos DiftAdjustment W Rmatix Kinematics &Nomalization [RMatix] Oientation Rmatix [GPS] Adjustment Eo Eo Yaw Couse PI Contolle Dift Detection Pitch Roll [Acceleometes] Gavity Figue 1 Block diagam of DCM No doubt you ae wondeing what a otation goup is, and why it should be espected. You also might be wondeing how you can use DCM fo contol and navigation. You also might have the same questions that UFO-MAN asked on the subject afte he ead Mahoney s pape, so we will stat with those questions: What is a quatenion and why do we use that instead of vecto notation? What is meant by a otation goup? What is a otation matix? What does it mean to maintain othogonality of the otation matix? What is an anti symmetic matix? Can you biefly explain kinematics in this otation matix context? Can you biefly explain dynamics in this otation matix context? DCM 3 Daft: 5/17/2009

4 All of this is concened with otations. Physically, what we ae tying to do is epesent the oientation of ou aicaft with espect to the eath as a otation. Thee ae seveal ways to do this. Mahony's pape discusses two altenate ways, otation matices and quatenions. Both appoaches ae simila in motivation, they ae epesenting otations without appoximations and without singulaities. uatenions have the advantage of equiing only 4 values, while otation matices have 9. Rotation matices have the advantage of being a natual fit to contol and navigation. We chose otation matices as having a slight advantage, and fo being moe familia to us. xb yb zb xe ye ze R = xe ye R -1 = xb yb ze zb A otation matix descibes the oientation of one coodinate system with espect to anothe. The columns of the matix ae the unit vectos in one system as seen in the othe system. A vecto in one system can be tansfomed into the othe system by multiplying it by the otation matix. The tansfomation in the evese diection is accomplished with the invese of the otation matix, which tuns out to be equal to its tanspose. (The tanspose is just the swap of ows and columns.) Unit vectos ae useful in the contol and navigation computations, because thei length is one. Theefoe they can be used in dot and coss poducts to obtain the sine o cosine of vaious angles. y = R.y z y y = R -1.y R -1 = t R x R.R -1 = I t R.R = I z y x DCM 4 Daft: 5/17/2009

5 As you plane flies along, it is possible to descibe its motion with a tanslation (movement of its cente of gavity) and a otation (change in oientation aound its cente of gavity). Its oientation with espect to the eath can be descibed by specifying a otation about an axis. By stating with the plane level and pointing in a standad diection and applying the otation, you will place the plane in its actual oientation. Any oientation can be descibed as a otation fom the standad position. A otation goup is the goup of all possible otations. It is called a goup because any two otations in the goup can be composed to poduce anothe otation in the goup, evey otation has an invese otation, and thee is an identity otation. That is the definition. Howeve, the way that we like to think about it as being a goup is that you can wind up going aound a complete cicle and aiving back whee you stated. The otation goup is closed. The eason that the otation goup should be espected is that by doing that, you make the fewest appoximations and ae able to pefom contol and navigation with the plane in any oientation, including upside down and pointing vetical. You can do aeobatics without making any appoximations. The basic idea is that the otation matix that defines the oientation of you aicaft can be maintained by integating the nonlinea diffeential equation that descibes the kinematics of the otation. (We will pesent the nonlinea diffeential equation shotly, and explain why it is nonlinea.) Kinematics is concened with the geomety of the otation of a igid body, and how the otation tansfoms one igid configuation into anothe configuation. This is done by ecognizing that the integation can be accomplished via a seies of matix compositions. By matix composition we simply mean multiplying two otation matices togethe. It can be shown that the esulting matix epesents the net otation that esults fom applying the two otations in sequence that each of the matices epesents. Howeve, numeical integation intoduces numeical eos, and does not poduce the same esult that symbolic integation. An exact symbolic integation of the exact gyo signals will poduce the exactly coect otation matix. Numeical integation, even if we had the exact gyo signals, will intoduce two sots of numeical eos: Integation eo. Numeical integation uses a finite time step and data that is sampled at a finite sampling ate. Depending on the method that you use to do the integation, you ae making cetain assumptions about what is happening between data samples. The method that we use in ou implementation assumes that the otation ate is constant ove the time step. This intoduces an eo that is popotional to the otational acceleation. uantization eo. No matte what epesentation you use fo the values, the digital epesentation is finite, so thee is a quantization DCM 5 Daft: 5/17/2009

6 eo, stating at the analog to digital convete, and building wheneve you pefom a calculation that does not peseve all of the bits of the esult. One of the key popeties of the otation matix is its othogonality, which means that if two vectos ae pependicula in one fame of efeence, they ae pependicula in evey fame of efeence. Also, that the length of a vecto is the same in evey fame of efeence. Numeical eos can violate this popety. Fo example, since the ows and columns ae supposed to epesent unit vectos, thei magnitude should be equal to one, but numeical eos could cause them to get smalle o lage. Eventually they could shink to zeo, o go to infinity. The ows and columns ae supposed to be pependicula to each othe, numeical eos could cause them to "lean" into each othe, as shown below: The otation matix has 9 elements. Actually, only 3 of them ae independent. The othogonality popety of the otation matix in mathematical tems means that any pai of columns (o ows) of the matix ae pependicula, and that the sum of the squaes of the elements in each column (o ow) is equal to 1. So, thee ae 6 constaints on the 9 elements. xb = 1 yb = 1 zb = 1 xb yb xb zb R = xb yb zb xe ye ze yb zb An antisymmetic matix is a matix in which each element in the matix is equal to the negative of the element with swapped ow and column index. So, fo example, if the element in the fist ow, thid column is 0.5, then the DCM 6 Daft: 5/17/2009

7 element in the thid ow, fist column must be Also, the elements on the diagonal of an antisymmetic matix must be zeo. It tuns out that a small otation can be descibed with an antisymmetic matix as shown below: 0 a b -a 0 c -b -c 0 In ou case, kinematics is concened with the implications of igid body otation. It esults in a nonlinea diffeential equation that descibes the time evolution of the oientation of the body in tems of its vecto otation ate. The diection cosine matix is all about kinematics. Dynamics in ou case is the application of Newton's laws to descibe the time ate of change of the otation ate vecto in tems of the applied toques. By the way, the dynamics in Mahony's pape ae NOT accuate fo planes, they wee concened mainly with helis and vetical take-offs. Mahony's pape descibes how to implement a combined oientation measuement and contol algoithm. What Paul and I ae doing involves kinematics only. We have completely ignoed dynamics fo now. The kinematics (otation matix) by itself is vey useful fo poviding a basis fo contol and navigation of model aiplanes. You ae pobably still wondeing how to use DCM. Contol and navigation can be accomplished with DCM entiely in Catesian coodinates using vecto coss poducts and dot poducts. Fo example, at a high level, hee is how you accomplish these fou contol and navigation calculations. 1. To contol the pitch of an aicaft, you need to know the pitch attitude of the aicaft, which you can find by taking the dot poduct of the oll axis of the aicaft with the gound vetical. 2. To contol the oll of an aicaft, you need to know the bank attitude of the aicaft, which you can find by taking the dot poduct of the pitch axis of the aicaft with the gound vetical. 3. To navigate, you need to know the yaw attitude of the aicaft with espect to the diection that you want to go, which you can find by taking the coss poduct of the oll axis of the aicaft with a vecto in the diection that you want to go. This woks even if you ae upside down. To find out if you aicaft might be pointing in the opposite DCM 7 Daft: 5/17/2009

8 diection than you want to go, take the dot poduct of the oll axis with the desied diection vecto. If it is negative, the aicaft is moe than 90 degees off couse. 4. To find out if the aicaft is upside down, examine the sign of the dot poduct of the aicaft yaw axis with the vetical. If it is less than zeo, the aicaft is upside down. 5. To find out the tuning ate of the aicaft aound the vetical eath axis, tansfom the gyo otation vecto to the eath fame of efeence, and take the dot poduct with the vetical axis. We now get deepe into the details of the theoy. Axis conventions To descibe the motion of an aiplane it is necessay to define a suitable coodinate system. Fo most poblems dealing with aicaft motion, two coodinate systems ae used. One coodinate system is fixed to the eath and may be consideed fo the pupose of aicaft motion analysis to be an inetial coodinate system. The othe coodinate system is fixed to the aiplane and is efeed to as a body coodinate system. Figue 2 shows the two ight-handed coodinate systems. θ xb ye ψ ψ φ yb xe zb φ θ ze Figue 2 Body fixed fame and eath fixed fame The oientation of the aiplane is often descibed by thee consecutive otations, whose ode is impotant. The angula otations ae called the Eule angles. The oientation of the body fame with espect to the fixed eath fame can be detemined in the following manne. Imagine the aiplane to be DCM 8 Daft: 5/17/2009

9 positioned so that the body axis system is paallel to the fixed fame and then apply the following otations: 1. Rotate the body about its zb axis though the yaw angle ψ 2. Rotate the body about its yb axis though the pitch angle θ 3. Rotate the body about its xb axis though the oll angle φ xb yb zb Figue 3 Body axes coodinate system Diection cosine matices Cetain types of vectos, such as diections, velocities, acceleations, and tanslations, (movements) can be tansfomed between otated efeence fames with a 3X3 matix. We ae inteested in the plane fame of efeence and the gound fame of efeence. It is possible to otate vectos by multiplying them by a matix of diection cosines: x = y = a vecto, such as a diection, velocity o acceleation z = a vecto measued in the fame of efeence of the plane P G xx R = yx zx = R G = a vecto measued in the fame of efeence of the gound P xy yy zy xz yz zz = otation matix Eqn. 1 DCM 9 Daft: 5/17/2009

10 The elation between the diection cosine matix and Eule angles is: R = cosθ cosψ sinφ sinθ cosψ cosφ sinψ cosφ sinθ cosψ + sinφ sinψ Eqn. 2 cosθ sinψ sinφ sinθ sinψ + cosφ cosψ cosφ sinθ sinψ sinφ cosψ sinθ sinφ cosθ cosφ cosθ Equation 1 and equation 2 expesse how to otate a vecto measued in the fame of efeence of the plane to the fame of efeence of the gound. Equation 1 is expessed in tems of diection cosines. Equation 2 is expessed in tems of Eule angles. In equation 1, each component of the vecto in the gound fame is equal to the dot poduct of the coesponding ow of the otation matix with the vecto in the plane fame. Nine multiplies and six additions ae equied to compute the otation. Equation 3 is a estatement of equation 1, with the matix multiplication expanded in tems of the elements of the vectos and the matix. Gx Gy Gz = = = xx yx zx Px Px Px xy yy zy Py Py Py + + xz + yz zz Pz Pz Pz Eqn. 3 Note that the R matix is not necessaily symmetic. The thee columns of the R matix ae the tansfomations of the thee axis vectos of the plane to the gound fame of efeence. The thee ows of the R matix ae the tansfomations of the thee axis vectos of the gound coodinate system to the plane fame of efeence. The R matix contains all the infomation needed to expess the oientation of the plane with espect to the gound. The R matix is also called the diection cosine matix, because each enty is the cosine of the angle between an axis of the plane and an axis on the gound. Although it would appea that thee ae 9 independent paametes in the R matix, thee ae eally only 3 independent ones, because of the six so-called othogonality (also known as nomalization) conditions: the thee column vectos ae mutually pependicula and the magnitude of each column vecto is equal to one. The tanspose of any matix, and the otation matix in paticula, indicated T as R, is fomed by intechanging ows and columns. In geneal, the invese 1 of a squae matix, if it exists, is indicated as R. The invese of a matix times the matix poduces the identity matix. (The identity matix has all ones on the diagonal, and all zeos eveywhee else. Multiplying any matix by the identity matix leaves it unchanged. In the case of otation matices, it tuns out that the tanspose of the R matix is equal to its invese: DCM 10 Daft: 5/17/2009

11 R 1 P = R = R T 1 = G xx xy xz = R T yx yy yz G zx zy zz Eqn. 4 The eason that the invese of the otation matix is equal to its tanspose is because of the symmety of the situation. The elements of the otation matix ae the cosines between pais of axis, one in the plane fame, and one in the gound fame. The invese situation is equivalent to exchanging the oles of the gound and plane fame of efeence, which is the same as intechanging ows and columns, which is the same as the tanspose. Also, the fact that the invese is equal to the tanspose is consistent with the othogonality conditions, which can be expessed in matix notation as: RR T = R T 1 R = I = Eqn. 5 Equation 5 can be used to pove that the invese of R is R tanspose, by multiplying the equation by the invese of R, o by the invese of R tanspose. A vey useful popety of the otation matix is that we can compose otations. We can multiply seveal otations matices togethe, and get a otation matix that is equivalent to applying all of the otations in succession. We have to be caeful to apply the otations in succession on the left side of what we aleady have. Fo example, if we have thee otation matices, fom oientation A to oientation B, fom B to C, and fom C to D, we can compute the otation matix that will go fom oientation A to oientation D accoding to: R R R R DA BA CB DC = R DC R CB R BA = otation matix fom A to B = otation matix fom B to C = otation matix fom C to D Eqn. 6 R DA = otation matix fom A to D The eason that we have to be caeful about the sequence of opeations when multiplying otations matices is that matix multiplication is NOT commutative. That is, the ode of matix multiplication mattes vey much.. This is consistent with otations, which ae not commutative eithe. Fo example, conside what happens if a plane pitches aound its own pitch and oll axes by 90 degees each. The ode vey much mattes. Suppose that is pitches up by 90 degees, followed by a oll of 90 degees. At that point the plane will be taveling vetically. Howeve, if it olls fist, and then banks, it will be taveling in the hoizontal plane. DCM 11 Daft: 5/17/2009

12 Finally, thee is a useful identity that applies to matices in geneal, and to otation matices in paticula. The tanspose of the poduct of two matices is equal to the poduct of the tansposes of the matices, with the two matices swapped: ( AB) T = B T A T A,B ae matices Eqn. 7 Vecto dot and coss poducts Two vey useful vecto poducts that we will use in computing DCM and in using its elements fo navigation and contol ae the dot poduct and the coss poduct. The dot poduct of two vectos A and B, is a scala computed by pefoming a matix multiplication of a A as a ow vecto with B as a column vecto poducing: A B = A T B = B x [ A x Ay A ] z B y = Ax Bx + AyBy + Az Bz B z Eqn. 8 It tuns out that the vecto dot poduct poduces a esult that is equal to the poduct of the magnitudes of the two vectos, times the cosine of the angle between them: ( ) A B = A B cos Eqn. 9 We note that the dot poduct is commutative: A B = B A The coss poduct of two vectos A and B, is a vecto whose components ae computed by: θ AB ( A B) x = AyBz Az By ( A B) y = Az Bx Ax Bz ( A B) = Ax By AyBx z Eqn. 10 The coss poduct is pependicula to both of its vecto factos and its magnitude is popotional to the magnitudes of the vectos times the sine of the angle between them: DCM 12 Daft: 5/17/2009

13 w k θ v u w = u v = u. v.sin(θ).k w = u. v.sin(θ) k : unit vecto othogonal to the plane defined by u and v Stated anothe way: ( A B) A = ( A B) B A B = A B sin( θ AB ) = 0 We note that the coss poduct is anti-commutative. A B = B A Eqn. 11 Computing diection cosines fom gyo signals With the peliminaies out of the way, we now move on to the cental concept of the DCM algoithm: the nonlinea diffeential equation that elates the time ate of change of the diection cosines to the gyo signals. Ou goal is to compute the diection cosines without making any appoximations that violate the nonlineaity of the equations. Fo the moment, we assume that the gyo signals have no eos. Late on we will addess the issue of gyo dift. Unlike otating mechanical gyos, which stay fixed in space while the aicaft otates aound them, electonic ate gyos otate with the aicaft, poducing signals popotional to the otation ate. Since otations do not commute, and the sequence of otations matte, we cannot get by with simply integating the gyo ate signals to get angles, that will not wok. What we have to do is look to the kinematics of otations to see what we need to do to get the coect answe. A well known esult of kinematics is that the ate of change of a otating vecto due to its otation is given by: ( t) d = ω() t () t dt ω() t = otation ate vecto We make the following obsevations: Eqn. 12 DCM 13 Daft: 5/17/2009

14 1. The diffeential equation is nonlinea. The otation vecto input is (coss) multiplied by the vaiable that we ae tying to integate. Theefoe, any linea appoach will be only an appoximation. 2. Both vectos must be measued in the same efeence fame. 3. Because the coss poduct is anticommutative, we could evese the ode and change the sign. If we know the initial conditions and the time histoy of the otation vecto, we can numeically integate equation 11 to tack the otating vecto: d t 0 ( t) = ( 0) + dθ( τ ) ( τ ) 0 θ( τ ) = ω( τ ) ( 0) = dθ t dτ stating value of the vecto ( τ ) ( τ ) = change in the vecto Eqn. 13 Ou stategy is going to be to apply equation 13 to the ows o the columns of the R matix, teating them as otating vectos. The fist snag that we un into is that the vectos that we want to tack, and the otation vecto, ae not measued in the same efeence fame. Ideally, we would like to tack the axes of the aicaft in the eath fame of efeence, but the gyo measuements ae made in the aicaft fame of efeence. Thee is an easy solution to the issue by ecognizing the symmety in the otation. In the fame of efeence of the plane, the eath fame is otating equal and opposite to the otation of the plane in the eath fame. So we can tack the eath axes as seen in the plane fame by flipping the sign of the gyo signals. As a matte of convenience, we can flip the sign back, and intechange the factos in the coss poduct: eath dθ ( t) ( 0) + ( τ ) dθ( τ ) = eath ( τ ) = ω( τ ) eath t 0 eath dτ () t = one of the eath axes, as viewed fom the plane Eqn. 14 The vectos in equation 14 ae the ows of the R matix in equation 1. The next question is how to conveniently implement equation 14. We take the same matix appoach that Mahoney [1] uses. We stat by going back to the diffeential fom of equation 14: eath( t + dt) = eath( t) + eath( t) dθ( t) Eqn. 15 dθ() t = ω()dt t Thee is one moe thing that we need to do, in anticipation of the dift cancellation that we will be doing late on. We need to add the coection otation ate that comes out of the popotional plus integal dift DCM 14 Daft: 5/17/2009

15 compensation feedback contolle to the measuement that the gyos make, to poduce ou best estimate of the tue otation ate: ω ω ω () t = ωgyo( t) + ωcoection( t) () t = gyo coection thee axis gyo measuements () t = gyo coection Eqn. 16 Late on we will explain the details of computing the gyo coection vecto. Basically, the GPS and acceleomete efeence vectos that we have ae used to compute a otational eo, which is fed into the computation though the feedback contolle, and back into the otation update equation via equation Eqn. 16. When we epeat equation 14 fo each of the eath axes, we can put the esult into a convenient matix fom: R ( t + dt) = R( t) dθ = ω dt x dθ = ω dt y dθ = ω dt z x y z 1 dθ z dθ y dθ 1 dθ x z dθ y dθ x 1 Eqn. 17 Equation 17 is a ecipe fo updating the diection cosine matix fom gyo signals. It is equivalent to Manhoney s esult. The values of 1 on the diagonal of the matix in equation 17 epesent the fist tem in equation 15. The smalle, off-diagonal elements epesent the second tem in equation 15. Equation 17 is implemented numeically by epeated matix multiplications, with shot time steps. Each matix multiplication equies 27 multiplications and 18 additions. It maps well to the dspic30f4011, which has hadwae esouces to pefom matix multiplication efficiently. It can be pefomed on CPUs that do not have matix suppot, in which case it is ecommended to use intege aithmetic. The only appoximation that equation 17 makes is that the time step is shot enough so that the R matix does not change much fom step to step. A typical time step is aound seconds, duing which an aicaft otating at aound 60 degees pe second otates appoximately adians, which tanslates to a maximum change in any of the R matix coefficients of aound 2%. Thus, the second ode tems that ae being ignoed ae on the ode of 0.02%. Tests and simulations have shown that implementation of equation 15 by itself, with gyos with modest pefomance, yields vey accuate esults that achieve vey low dift, on the ode of a few degees pe minute. The dift is so low that it is a simple matte to adjust fo it without compomising pefomance. Howeve, by itself, equation 15 will eventually accumulate DCM 15 Daft: 5/17/2009

16 numeical ound-off and gyo dift, offset, and gain eos. In the next two sections we will explain how to cancel the eos. Renomalization Numeical eos will gadually educe the othogonality conditions expessed by equation 5 to appoximations athe than identities. In effect, the axes in the two fames of efeence no longe descibe a igid body. Fotunately, numeical eo accumulates vey slowly, so it is a simple matte to stay ahead of it. We call the pocess of enfocing the othogonality conditions enomalization. We devised seveal ways that it could be done. Simulations showed they all woked quite well, so in the end, we settled on the simplest appoach. It woks as follows. Fist we compute the dot poduct of the X and Y ows of the matix, which is supposed to be zeo, so the esult is a measue of how much the X and Y ows ae otating towad each othe: X = xx xy xz Y = eo = X Y = X T yx yy yz Y = [ ] xx xy xz yx yy yz Eqn. 18 We appotion half of the eo each to the X and Y ows, and appoximately otate the X and Y ows in the opposite diection by coss coupling: xx xy xz yx yy yz othogonal othogonal = X = Y othogonal othogonal eo = X Y 2 eo = Y X 2 Eqn. 19 You can veify that the othogonality eo is geatly educed by substituting equation 19 into 18, keeping in mind that the magnitude of each ow and column of the R matix is appoximately equal to one. Appotioning the eo equally to each vecto yields a lowe esidual eo afte the coection than if the eo wee assigned entiely to one of the vectos. The next step is to adjust the Z ow of the matix to be othogonal to the X and Y ow. The way we do that is to simply set the Z ow to be the coss poduct of the X and Y ows: DCM 16 Daft: 5/17/2009

17 xx xy xz othogonal = Z = X Y othogonal othogonal othogonal Eqn. 20 The last step in the enomalization pocess is to scale the ows of the R matix to assue that each has a magnitude equal to one. One way we could do that is to divide each element of each ow by the squae oot of the sums of the squaes of the elements in that ow. Howeve, thee is an easie way to do that, by ecognizing that the magnitudes will neve be much diffeent than one, so we can use a Taylo s expansion. The esulting magnitude adjustment equations fo the ow vectos ae: X Y Z nomalized nomalized nomalized 1 = 2 1 = 2 1 = 2 ( 3 X X ) othogonal ( 3 Y Y ) othogonal othogonal othogonal Y X othogonal othogonal ( 3 Z othogonal Z othogonal ) Z othogonal Eqn. 21 What equation 21 says to do to adjust the magnitude of each ow vecto to one, is to subtact the dot poduct of the vecto with itself (the squae of the magnitude), subtact fom thee, multiply by ½, and multiply each element of the vecto by the esult. Thee ae not that many multiplies and additions in the nomalization pocess. Thee ae no divisions o squae oots. We pefom the calculation fo each step of the integation, evey seconds. Dift cancellation Although the gyos pefom athe well, with an uncoected offset on the ode of a few degees pe second, eventually we have to do something about thei dift. What is done is to use othe oientation efeences to detect the gyo offsets and povide a negative feedback loop back to the gyos to compensate fo the eos in a classical detection and feedback loop, as shown if Figue 1. The steps ae: 1. Use oientation efeence vectos to detect oientation eo by computing a otation vecto that will bing the measued and computed values of efeence vectos into alignment. 2. Feed the otation eo vecto back though a popotional plus integal (PI) feedback contolle to poduce a otation ate adjustment fo the gyos. (A PI egulato is a special case of a commonly used feedback egulato called a PID egulato. The D stands fo deivative. In ou case, we do not need the deivative tem.) 3. Add (o subtact, depending on you sign convention fo the otation eo) the output of the PI contolle to the actual gyo signals. DCM 17 Daft: 5/17/2009

18 The main equiement fo an oientation efeence vecto is that it does not dift. Its tansient pefomance is not that impotant because it is the gyos that povide the tansient fidelity fo the oientation estimate. Ou two efeence vectos ae supplied by GPS and acceleometes. Magnetometes ae also useful, paticulaly fo yaw contol of hoveing applications, but fo aicaft that fly geneally in the diection that they ae pointed, a GPS will do just fine. If you use a magnetomete, to povide a vecto efeence you should use a thee axis magnetomete. Low cost thee axis magnetometes ae commecially available. We use acceleometes to povide a efeence vecto fo the Z axis of the aiplane. Details will be given in a sepaate section. We use the GPS as a efeence fo the hoizontal pojection of the X axis (oll axis) of the plane. Ou two efeence vectos happen to be pependicula to each othe. That is convenient, but not absolutely necessay. Fo eithe of the two efeence vectos, the oientation eo is detected by taking the coss poduct of the measued vecto with the vecto that is estimated by the diection cosine matix. The coss poduct is paticulaly appopiate fo two easons. Its magnitude is popotional to the sine of the angle between the two vectos, and its diection is pependicula to both of them. So it epesents an axis of otation, and an amount of otation, that would be needed to otate the measued vecto to become paallel to the estimated vecto. In othe wods, it is equal to the negative of the oientation otational eo. By feeding it back to the gyos though the PI contolle, the estimated oientation is gadually foced to tack the efeence vectos, and gyo dift is cancelled. The coss poduct of a measued efeence vecto with a coesponding vecto computed fom the diection cosine matix is an indication of the eo. It is appoximately equal to the otation that would have to be applied to the efeence vecto to bing it into alignment with the computed vecto. We ae inteested in the amount of otation coection that we need to apply to the diection cosine matix, which is equal to the negative of the eo otation. So, it is convenient to compute the coection by intechanging the ode in the coss poduct. The coectional otation is equal to the coss poduct of the vecto estimated by the diection cosines with the efeence vecto. We use a popotional plus integal feedback contolle to apply the otation coection to the gyos, because it is stable and because the integal tem completely cancels gyo offset, including themal dift, with zeo esidual oientation eo. The way that the efeence vecto eos map back onto the gyos is done via the diection cosine matix, so that the mapping depends on the oientation of the IMU. Fo example, the GPS efeence vecto might coect eithe the X, Y, Z, o combinations of X, Y and Z axis gyo signals, depending on the oientation of the axes with espect to the eath fame. DCM 18 Daft: 5/17/2009

19 We will now get into moe detail fo the two efeences that we ae using. GPS GPS povides a dift-fee efeence vecto fo the yaw oientation of the plane. The only eason that we do not use GPS by itself fo yaw infomation is that the tansient esponse of the gyos is much faste than that of the GPS. Instead, we use GPS as a efeence vecto to cancel gyo dift and achieve a yaw lock. Two of the majo sevices povided by a GPS adio ae epoting of location and velocity magnitude and diection. The GPS detemines its location and velocity fom the signals that it eceives fom obiting satellites, and sends the infomation out though its seial inteface. Fo most GPS eceives, thee ae two data fomats, NMEA and binay. NMEA is a comma delimited, human eadable standadized ASCII fomat. In the binay inteface, binay values ae tansmitted as sequences of the ones and zeos of thei intenal binay epesentation. The binay inteface povides some additional infomation that is not available in the NMEA inteface. The GPS must move in ode to give diection infomation. Othewise, thee is no way to detemine the oientation of the GPS antenna. The velocity vecto epoted by GPS is the change in position of the antenna in 1 second. Thee ae seveal ways a GPS might do the computation, but fo all methods, the GPS must move. Thee ae two diffeent coodinate systems fo GPS units to epot location and velocity. One system epots longitude, latitude, altitude, velocity ove gound, and couse ove gound. Couse ove gound is the angle of the couse measued clockwise fom the noth. Inteestingly enough, this is the same angle as measued in the mathematical sense (counte clockwise) aound the Z axis in the body efeence fame of the plane, with the Z axis pointing down. In this system, vetical velocity is available though the binay inteface. The othe system, ECEF (eath-centeed, eath-fixed), epots X, Y, Z position and velocity, with the oigin of the ight-handed X, Y, Z coodinate system at the cente of the eath. A GPS delives its infomation as a continuous sequence of epots, typically once evey second (1 Hz), though thee is a tend towad highe epoting ates, with 5 times a second (5 Hz) becoming moe common. Howeve, a highe epoting ate does not necessaily lead to bette pefomance, because of the limitations imposed by the dynamics of the GPS intenal signal pocessing. Thee ae seveal factos that should be kept in mind in consideing GPS dynamics: DCM 19 Daft: 5/17/2009

20 1. Repoting latency. Unde cetain cicumstances, fo some GPS units it may take as long as 12 seconds fo the computed data to be tansmitted. 2. Filteing. All GPS units pefom some sot of filteing to impove the accuacy of position and velocity estimation. This will esult in a smoothing effect on the data when the GPS changes its velocity o position, so that the new infomation is not seen instantly, but athe becomes appaent gadually. 3. Tack smoothing and static navigation. Many types of GPS adios povide a tack smoothing option to ignoe sudden changes in position o velocity. This is useful fo automotive applications to pevent changes fom being seen as the esult in changes in the satellite signals, such as when collection of satellites that ae being used changes. They also povide a static navigation option so that vaiation in the appaent location is suppessed when the velocity falls below a cetain value. This is also useful fo automotive applications. It is not likely that you will evey un into tack smoothing o static navigation, because the factoy defaults ae to tun these options off, but you should be awae of them. Howeve, epoting latency and filteing must be taken into account. By epoting latency we mean a simple time delay between when the GPS measues position and velocity, and when it appeas in the sequence of messages. Usually this delay is the epoting time peiod. Fo example, if you GPS is epoting at 5 Hz, the epoting latency is typically 0.2 seconds. Howeve, it could be much lage than that if you ae not caeful. One of us (Bill) had the bad luck of stumbling into a 12 second latency with a epoting ate of 1 Hz. It tuned out that the 12 second delay was tiggeed by using a combination of 4800 baud and the binay inteface. It was educed to a 1 second latency by changing the baud ate to 19,200. Chances ae that you will not un into this effect, but be awae that it exists. If you use the binay inteface, you should use a baud ate of 19,200 o geate. In addition to a simple latency, you will geneally also un into a delay caused by intenal filteing done by the GPS. All GPS units pefom some sot of filteing of the data by the vey natue of how they do thei computations. Thee is an inheent compomise in any system between accuacy and tansient esponse. The moe accuate you want to know something, the longe it will take to estimate it. In most units, the filteing shows up as a smoothing of the data. Typically, the dynamic esponse of many types of GPS is a simple exponential esponse with a 1 second time constant, so that it takes about 3 seconds to fully espond to a step change. If you ignoe the GPS dynamics, thee will be a small eo intoduced into you navigation calculations duing a tun. One of us (Paul) saw that it is possible to compensate fo this small eo by intoducing a filte between the diection cosine matix and the input to the yaw dift coection. [Do we need a figue?]. DCM 20 Daft: 5/17/2009

21 That way, the dynamics of the two vectos used in the estimation of the yaw eo ae matched. It is often assumed that a GPS with a high epoting ate, such as 5 Hz, will povide bette dynamic pefomance that on with the most common epoting ate, 1 Hz. Howeve it is not necessaily the case that the highe epoting ate will povide bette dynamic esponse. Cetainly, its latency will be less. Howeve, thee is still the issue of the filte dynamics, which will geneally tun out to be the limiting effect. The GPS hoizontal couse ove gound signal has zeo dift ove the long tem, and can be used as a efeence vecto to achieve yaw lock fo the IMU. We consideed also including the vetical velocity fom the GPS, but decided against it, in favo using the acceleometes fo vetical infomation. The assumption is made that the aicaft is moving in the diection that it is pointing. Any tansient eos in that assumption do not mateially affect pefomance. Howeve, stong winds, paticulaly coss winds, do violate this assumption. Thee ae two appoaches that you can take. One appoach is to somehow compute the wind vecto fom available infomation. We ae continuing to wok on that. The othe appoach is to use modeate feedback gains. The diffeence between the diection the aicaft is pointing and the diection that it is moving will show up as an eo at the input to the dift coection feedback contolle. The esult will be that DCM will adapt to the wind, and otate the plane the amount equied to keep it moving along the desied couse ove gound. The following figue shows how the yaw coection is computed: DCM 21 Daft: 5/17/2009

22 xb p : pojection of xb on eath xy plane ψ estimated yaw COG: GPS couse ove gound vecto ψ m measued yaw θ xb COG ye xb p ψ m ψ φ yb xe (xb yb zb) body fame (xe ye ze) eath fame zb φ θ ze ψ yaw angle θ pitch angle φ oll angle xb = 1 xb p = cos(θ) xb p COG = YawCoectionGound The otational eo between the GPS couse ove gound vecto, and the pojection on the hoizontal plane of the oll axis (X) of the IMU is an indication of the amount of dift. The otational coection is the Z component of the coss poduct of the X column of the R matix and the couse ove gound vecto. Fist, we fom the efeence vecto fom the nomalized hoizontal velocity vecto. This can be done by simply taking the cosine and sine of the couse ove gound angle, in the eath fame of efeence: COGX = cos( cog) Eqn. 22 COGY = sin( cog) We then compute yaw coection: YawCoectionGound = COGY COGX Eqn. 23 Howeve, equation 23 yields the yaw coection in the eath fame of efeence. In ode to adjust the gyo dift, we will need to know the coection vecto in the aicaft (body) fame of efeence. To compute that we must multiply the yaw coection in the gound fame of efeence by the Z ow of the R matix: xx zx YawCoect ionplane = YawCoectionGound zy Eqn. 24 zz yx DCM 22 Daft: 5/17/2009

23 The yaw coection vecto poduced by equation 24 will be combined with oll-pitch coection computed fom the acceleometes into a total vecto that is used to compensate fo dift. Details of that computation will be given afte we discuss how the acceleometes ae used. Thee ae thee conditions elative to yaw dift compensation that ague fo a lage weighting of the yaw coection, to enable a apid esponse to yaw eo. The fist condition is initial yaw lock. When the algoithm stats up, it has no way of knowing what diection the boad is pointing. Even if it did, duing the time it waits fo GPS lock, it will be difting, and even afte GPS is locked, the GPS epoted couse ove gound will be andom numbes befoe the plane is launched. By giving the yaw dift coection a lage weight, yaw lock can be achieved shotly afte takeoff. The second condition is winds. If the plane tavels fo a long time in a coss wind, the wind will be teated as a gyo offset. If the plane then makes a 180 degee tun, fo a while the DCM algoithm will tun the plane by the opposite angle that it would need to compensate fo the wind. The thid condition is when the plane is taveling vetically. Duing that time the X axis of the plane is vetical, and equation 23 yields zeo. Fo these easons, it is best to use a lage weight fo the yaw dift coection. Acceleometes Acceleometes ae used fo oll-pitch dift coection because they have zeo dift. We do have to woy about centifugal acceleation, but that can be accounted fo, and will be discussed shotly. When one of us (Bill) built his fist boad, he had hopes that acceleometes could be used by themselves fo oll-pitch contol. But they cannot, fo a numbe of easons. The main eason is that they measue a combination of acceleation and gavity. If they measued only gavity, they would be pefect. But they measue acceleation, too, and that can cause touble. Bill once tied to use acceleomete-only based pitch stabilization duing a hand launch of a sailplane. The acceleation of the launch fooled the contols into estimating that the plane was pitching up. The contols esponded by pitching the plane staight down. The way an acceleomete typically woks is that it measues the deflection of a small mass suspended by spings. The natual fequencies of the dynamics of the acceleomete ae high, so it does espond quickly. The deflection depends on the total foce on the mass, which is equal to its mass times the sum of the gavity vecto plus the acceleation vecto. (The usual sign convention fo acceleometes is such that they indicate gavity minus the acceleation.) DCM 23 Daft: 5/17/2009

24 So in addition to gavity, an acceleomete also measues acceleation. That should not be too supising, since that is what they ae called. Theefoe, an acceleomete is useful as a oll-pitch indicato only when the plane is not acceleating. The poblem is it is often acceleating. Some of the acceleations, such as centifugal acceleation, ae easy enough to compute and compensate fo without having a model of the dynamics of the plane. Howeve, thee is no easy way to sepaately compute the fowad acceleation. All is not lost. On aveage, a plane does not acceleate in the fowad diection. Thee ae times when it speeds up and when it slows down, but the acceleations cancel out. A plane cannot acceleate fo long in the fowad diection until aeodynamic dag pevents it fom going any faste. A plane cannot deceleate fo long without stopping and falling fom the sky. As long as we ae not depending on an acceleomete fo fast tansient esponse, we can use it fo oll-pitch coection of gyo dift, because the acceleomete does not dift. Thee ae many good acceleometes on the maket, most of them will wok just fine with the DCM algoithm. They ae not as citical as gyos, because any change in thei offsets does geneate an accumulated eo in the way that a gyo offset does. An acceleomete is a diect measuement of oientation, while a gyo is a measuement of the time ate of change of oientation. Thee ae a vaiety of inteface types, including analog voltage, pulse width modulation, and seveal standad communications intefaces. We chose an acceleomete with an analog voltage output as the simplest inteface. The geatest advantage of using diection cosines is that they wok fo any oientation of the plane, without any singulaities o special logic. Any oientation can be well-descibed by the 9 elements of the diection cosine matix. Since we will need to pefom the dift cancellation calculations fo any oientation of the plane, we will need to measue acceleation along all thee axes of the plane. This can be done with commecially available 3 axis acceleometes, o with 3 sepaate units. Befoe we can use the acceleomete infomation fo oll-pitch dift compensation, we must account fo the centifugal acceleation associated with changes in diection of the planes fowad velocity. Although a plane can acceleate o deceleate along the fowad diection fo a shot while, it can tun indefinitely. Fotunately, the infomation needed to compute the centifugal acceleation is eadily available. Centifugal acceleation is equal to the coss poduct of the otation ate vecto with the velocity vecto. We do not need an exact answe, only one that is accuate on aveage. On aveage, the plane moves in the diection that it is pointed. Theefoe, we can assume that the velocity vecto is paallel to the X axis of the plane. GPS gives us the magnitude of the velocity ove gound. Since gound is an inetial efeence fame, we can DCM 24 Daft: 5/17/2009

25 compute the velocity vecto in the plane (body) fame of efeence as being the velocity ove gound, in the X diection. In the plane (body) fame of efeence, we compute the centifugal acceleation as the coss poduct of the gyo vecto and the velocity vecto: A centifugal = ω velocity V = 0 0 gyo V Eqn. 25 Note that in equation 25, we only need to pefom two multiplications, because two of the elements of the velocity vecto in the plane (body) fame of efeence ae zeo. The usual sign convention fo commecial thee axis acceleometes is that the Z axis points down, and the downwad pull of gavity poduces a positive output. Theefoe, the output of the acceleometes is gavity minus the acceleation. To ecove an estimate of gavity that is adjusted fo centifugal acceleation, we need to add the centifugal acceleation estimate. Theefoe, the efeence measuement of gavity in the plane (body) fame is given by: g efeence = Acceleomete + ω Acceleometex Eqn. 26 Acceleomete = Acceleometey Acceleomete z In addition to the efeence measuement of gavity, we need an estimate based on the diection cosine matix. It is funished by the Z ow of the diection cosine matix, which is the pojection of the eath fame of efeence down axis along the axes of the plane (body) fame of efeence. gyo V DCM 25 Daft: 5/17/2009

26 (xb e yb e zb e ) estimated body fame ze e estimated eath z vecto (xb yb zb) body fame ze eath z vecto xb e xb θ ye xb p ψ φ yb xe yb e On this dawing the yaw angle is assumed coect but this is not geneally the case zb e zb φ ze e ze θ φ ψ yaw angle θ pitch angle φ oll angle The oll-pitch otational coection vecto in the body fame of efeence is computed by taking the coss poduct of the Z ow of the diection cosine matix with the nomalized gavity efeence vecto: zx RollPitchC oectionplane = zy zz g efeence Eqn. 27 Duing vey tight, continuous tuns, the acceleometes might become satuated. In othe wods, the actual acceleation might exceed the ange of the acceleomete. In that case, eo will be intoduced into the oll-pitch oientation estimate. The contols should be designed to avoid satuating the acceleometes. Similaly, the gyos can become satuated duing apid tuns. That can be avoided by including gyo tems in the contol feedback to limit the tuning ate. Feedback contolle Each of the otational dift coection vectos (yaw and oll-pitch) ae multiplied by weights and fed to a popotional plus integal (PI) feedback contolle to be added to the gyo vecto to poduce a coected gyo vecto that is used as the input to equation 17. (Now is a good time to go back an look at Figue 1.) The calculation poceeds as follows. Fist we compute a weighted aveage of the total of the otation coections. In ou case, thee ae just two coections, but in geneal thee could be moe: DCM 26 Daft: 5/17/2009