By: Prospero999 - prospero999  Quaternions and Aircraft Roll Demand Control* 2005-10-21 17:16 |
| Hi all, I'm writing a roll angle demand control loop for X-Plane (the flight simulation), but I'm coming up against what I suspect are the "usual problems" with a Euler Angles representation of attitude. To start with, I should mention that X-Plane outputs roll, pitch and yaw as Euler angles. Roll has range +/-180 degrees. Pitch has range +/-90 degrees. Yaw has range +/-180 degrees. I should also mention that the roll demand control loop's error signal is generated by calculating "desired roll angle minus actual roll angle." So, here's an example of the problem I'm having: Let's say that the aircraft starts out flying straight and level (pitch is about 0 degrees) and then pulls up into an almost vertical climb (pitch is almost 90 degrees). Well, now the roll value is just about to flip over from 0 to 180 degrees, and becomes VERY noisy, which in turn means my error signal is very noisy and my control loop doesn't function nearly as well as it does when the aircraft is flying straight and level with pitch around 0. I believe that quaternions may provide the answer, but I'm trying to work out how I actually use them to determine the roll error signal. I'm afraid I have only a little experience of quaternion mathematics. X-plane outputs the aircraft's attitude as Euler Angles, so if one's going to work with quaternions, I presume that the first stage would be to convert from a Euler Angles representation of attitude to a quaternion representation of attitude thusly (note that roll, pitch and yaw are in degrees, and q1w, q1x, q1y and q1z are the four elements of the quaternion, and DEGTORAD is Pi/180): q1w = Cos(roll * DEGTORAD / 2) * Cos(pitch * DEGTORAD / 2) * Cos(yaw * DEGTORAD / 2) + Sin(roll * DEGTORAD / 2) * Sin(pitch * DEGTORAD / 2) * Sin(yaw * DEGTORAD / 2) q1x = Sin(roll * DEGTORAD / 2) * Cos(pitch * DEGTORAD / 2) * Cos(yaw * DEGTORAD / 2) - Cos(roll * DEGTORAD / 2) * Sin(pitch * DEGTORAD / 2) * Sin(yaw * DEGTORAD / 2) q1y = Sin(roll * DEGTORAD / 2) * Cos(pitch * DEGTORAD / 2) * Sin(yaw * DEGTORAD / 2) + Cos(roll * DEGTORAD / 2) * Sin(pitch * DEGTORAD / 2) * Cos(yaw * DEGTORAD / 2) q1z = Cos(roll * DEGTORAD / 2) * Cos(pitch * DEGTORAD / 2) * Sin(yaw * DEGTORAD / 2) - Sin(roll * DEGTORAD / 2) * Sin(pitch * DEGTORAD / 2) * Cos(yaw * DEGTORAD / 2) And one can form the corresponding direction cosine matrix from the attitude quaternion like this: m11 = q1w ^ 2 + q1x ^ 2 - q1y ^ 2 - q1z ^ 2 m12 = 2 * (q1x * q1y + q1w * q1z) m13 = 2 * (q1x * q1z - q1w * q1y) m21 = 2 * (q1x * q1y - q1w * q1z) m22 = q1w ^ 2 - q1x ^ 2 + q1y ^ 2 - q1z ^ 2 m23 = 2 * (q1y * q1z + q1w * q1x) m31 = 2 * (q1x * q1z + q1w * q1y) m32 = 2 * (q1y * q1z - q1w * q1x) m33 = q1w ^ 2 - q1x ^ 2 - q1y ^ 2 + q1z ^ 2 And the roll Euler Angle can be extracted from the direction cosine matrix like this (RADTODEG is 180/Pi): roll = Atn2(m23, m33) * RADTODEG But that's as far as I can get. What I need to be doing is finding the roll error signal - i.e. the difference between desired roll and actual roll. Could some kind person help? Many thanks, Anthony |
By: Martin Baker - martinbaker RE: Quaternions and Aircraft Roll Demand Cont 2005-10-22 07:36 |
| Hi Anthony, I think one important issue is to minimise conversions between euler angles and quaternions or matrices because these conversions are costly in terms of performance and unstable around the singularities. Doing these conversions every time you go through the control loop would be horrible. We have to use quaternions or matrices when we want to combine rotations (we can only add euler angles if we are only changing one of pitch, roll or yaw or if the angles are very small) otherwise its probably best to stay in quaternions. So can you do the whole control loop in quaternions? Since adding angles is equivalent to multiplying quaternions then I suspect that finding the error would be equivalent to finding the angle difference which would be equivalent to dividing quaternions which is equivalent to multiplying by the conjugate. Martin |
| By: Prospero999 - prospero999 RE: Quaternions and Aircraft Roll Demand Control* 2005-10-22 08:07 |
| Hi Martin, Excellent - that last paragraph particularly was what I think I need! I will try to implement it and I'll get back to you. Many thanks, Anthony |
| By: Prospero999 - prospero999 RE: Quaternions and Aircraft Roll Demand Control* 2005-10-22 10:52 |
| Hi Martin, Well, I'm trying, but it's not quite right yet... Two things: 1) When you say, "Finding the error would be equivalent to finding the angle difference which would be equivalent to dividing quaternions which is equivalent to multiplying by the conjugate." Do you mean is equivalent to multipling by the inverse rather than by the conjugate? It's just that I read up on dividing quaternions, and they seemed to say inverse rather than conjugate... 2) Once the "difference" quarternion has been correctly calculated (if only!), you seem to imply that elements of this quarternion can be used "directly" to feed the autopilot with the roll angle error (i.e. without converting to matrix form and then extracting it from the matrix). How would I do this? Your help most appreciated! Anthony |
| By: Martin Baker - martinbaker RE: Quaternions and Aircraft Roll Demand Cont 2005-10-23 07:56 |
| > Do you mean is equivalent to multipling by the inverse rather than by the conjugate? It's just that I read up on dividing > quaternions, and they seemed to say inverse rather than conjugate... Yes, I should make that clearer on the website, the conjugate is the multiplicative inverse for quaternions. > 2) Once the "difference" quarternion has been correctly calculated (if only!), you seem to imply that elements of this > quarternion can be used "directly" to feed the autopilot with the roll angle error (i.e. without converting to matrix form > and then extracting it from the matrix). How would I do this? Since I don't know about your application I can only suggest general principles which may not be applicable to what you are doing. I was thinking that you had some control loop which, in a 2D case would be something like this: error angle = actual angle - required angle The equivalent is 3D might be: error quaternion = actual quaternion * conj(required quaternion) However, re-reading your message you are looking for the 'roll angle error' so perhaps you have to extract out the roll angle every time? but that does look horrible so its much better to formulate the problem in terms of quaternions or matrices if you can. Martin |
| By: Andy Goldstein - andygoldstein RE: Quaternions and Aircraft Roll Demand Cont 2005-10-23 09:07 |
| Hi, Anthony... As Martin says, once you're into quaternion form things are fairly simple. Given Q1 as your current attitude and Q2 as your desired attitude, the correction Qx you need (i.e., the difference between Q1 and Q2) is simply Q2 multiplied by the conjugate of Q1. To apply the correction gradually, you can divide the rotation angle represented by Qx by going back to the definition of a rotation quaternion - (w,x,y,z) where w is cos(rotation/2) and (x,y,z) is a vector of length sin(rotation/2) along the axis of rotation. Ultimately, to turn Qx back into control inputs for the plane you'll have to convert back to Eulers, since the flight controls of a plane ultimately control Euler angles. The good news is that the converted Euler angles are now in the plane's frame of reference, and as long as the error isn't too large the results should look pretty rational. Since you're dealing with X-Plane, I may be able to help since I helped Austin sort out his rotation math. X-Plane actually maintains the plane's attitude internally as a quaternion. If the quaternion form of the attitude is not available at the plugin interface (which I assume you're using), you should talk to Sandy Barbour or Ben Supnik about making it available. One thing you need to be aware of is that X-Plane's coordinate systems for the quaternion and for the XYZ space are not consistent, so the transformations between them are not the obvious ones. Please contact me directly at Andy.Goldstein@hp.com and I can send you code. Ultimately, though, we get back to what it is you're really trying to accomplish - in particular how you're arriving at and expressing the plane's desired attitude. You observed in your initial post that the roll angle becomes unstable in near vertical flight. This is absolutely correct and is characteristic of traditional Euler angles. The problem is that if you're expressing your desired roll angle as Eulers, that value has the same instability characteristics. So we get back to what the process is for establishing the plane's desired attitude to begin with. I don't have a good answer for this because I don't understand enough about what you're doing. - Andy |
| By: Prospero999 - prospero999 RE: Quaternions and Aircraft Roll Demand Control* 2005-10-23 09:41 |
| Many, many thanks Martin and Andy. Lots to think about there. I need a while to let it all sink in! Andy, by coincidence, I've been in correspondence with Sandy Barbour and Austin about related matters on the X-Plane.org forums. I've actually been putting off using Sandy's plugin SDK, though I mean eventually to use it, and so far have stuck with VB and UDP since my C++ is, well, shall we say rough at best?;) I have so many questions for you, but I'll try to think of framing them in a semi-intelligent way before emailing you! Cheers and thanks again, Anthony |
| By: Prospero999 - prospero999 RE: Quaternions and Aircraft Roll Demand Control* 2005-10-24 03:32 |
| Hi chaps, I'm still having a problem with using the quaternion division method to determine my roll angle error. I'm almost there, but something odd is going on. Here's the setup: actual attitude = w1, x1, y1, z1 desired attitude = w2, x2, y2, z2 So, to determine the error, we do the "quaternion equivalent" of "w2 minus w1" which is w2 mutiplied by the conjugate of w1: w3 = w2 * cw1 - x2 * cx1 - y2 * cy1 - z2 * cz1 x3 = x2 * cw1 + w2 * cx1 + y2 * cz1 - z2 * cy1 y3 = w2 * cy1 - x2 * cz1 + y2 * cw1 + z2 * cx1 z3 = w2 * cz1 + x2 * cy1 - y2 * cx1 + z2 * cw1 So I try this, and the resulting quaternion (w3, x3, y3, z3) isn't correct! But here's the odd thing: when I do "w1 minus w2" (i.e. w1 mutiplied by the conjugate of w2) using the following lines of code: w3 = w1 * cw2 - x1 * cx2 - y1 * cy2 - z1 * cz2 x3 = x1 * cw2 + w1 * cx2 + y1 * cz2 - z1 * cy2 y3 = w1 * cy2 - x1 * cz2 + y1 * cw2 + z1 * cx2 z3 = w1 * cz2 + x1 * cy2 - y1 * cx2 + z1 * cw2 ...I get the right error angles, but they are all the wrong sign. Which is I guess exactly what they should be... So how come w2 multiplied by the conjugate of w1 doesn't seem to work...? It *should* give me same numbers with the signs flipped, surely? Agh! I'm in Quaternion Hell! Any help much appreciated! Anthony |
| By: Andy Goldstein - andygoldstein RE: Quaternions and Aircraft Roll Demand Cont 2005-10-24 06:58 |
| Anthony, Reverse the order of multiplication and you'll get the right answer. You didn't give a formal expression of the quaternion product and I can't afford the time right now to reverse-compute it from your algebra. (Sorry - I know how tedious this stuff is. I've spent may hours cranking through multiplications.) However... Going back to basic quaternion theory, the way you rotate a vector P using quaternion Q is QP' = Q x QP x Q* where QP is the quaternion (0,P), Q* is the conjugate of Q and "x" denotes quaternion multiplication. To compose rotations, say Q1 and Q2 in that order, the math becomes QP' = Q2 x (Q1 x QP x Q1*) x Q2* Quaternion multiplication is associative and reversing the order yields you the conjugate, i.e. (Q1* x Q2*) = (Q2 x Q1)* So you can conclude that composing the rotations Q1 and Q2 (in that order) is Q2 x Q1. Relating my notation back to yours, the math you want is (conjugate of w1) x (w2) - Andy |
| By: Martin Baker - martinbaker RE: Quaternions and Aircraft Roll Demand Cont 2005-10-24 07:46 |
| Just to amplify what Andy said some people find it helpful to think of quaternions in terms of their relationship with the axis-angle representation: https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/notations/axisAngle/ With axis-angle reversing the angle reverses the direction of rotation. And reversing the direction of axis reverses the direction of rotation. And reversing the axis and the angle leaves the direction of rotation unchanged. similarly with quaternions, so, If w + i x + j y + k z rotates from A to B Then w - i x - j y - k z rotates from B to A And -w + i x + j y + k z rotates from B to A And -w - i x - j y - k z rotates from A to B (the last two don't apply to fundamental particles which have to rotate by 720 degrees to get back to where they started - When you get to the stage where quaternions are too simple for you its worth reading about quantum mechanics!). But for the sort of rotations we are talking about w + i x + j y + k z and -w - i x - j y - k z represent the same rotation. If the axis-angle way of looking at it doesn't help, ignore this message, different people like to think about quaternions in different ways. Martin |
| By: Nobody/Anonymous - nobody RE: Quaternions and Aircraft Roll Demand Control* 2005-10-24 11:26 |
| Thanks chaps, But something is still wrong. I must be bloody stupid. I am truly in Quaternion Hell. Andy, you said I should swap the order of multiplication, so when I originally said... "So, to determine the error, we do the "quaternion equivalent" of "w2 minus w1" which is w2 mutiplied by the conjugate of w1: w3 = w2 * cw1 - x2 * cx1 - y2 * cy1 - z2 * cz1 x3 = x2 * cw1 + w2 * cx1 + y2 * cz1 - z2 * cy1 y3 = w2 * cy1 - x2 * cz1 + y2 * cw1 + z2 * cx1 z3 = w2 * cz1 + x2 * cy1 - y2 * cx1 + z2 * cw1" ...should I actually be doing the following?: w3 = cw1 * w2 - cx1 * x2 - cy1 * y2 - cz1 * z2 x3 = cx1 * w2 + cw1 * x2 + cy1 * z2 - cz1 * y2 y3 = cw1 * y2 - cx1 * z2 + cy1 * w2 + cz1 * x2 z3 = cw1 * z2 + cx1 * y2 - cy1 * x2 + cz1 * w2 I tried this and it doesn't come out right:( Should it be right? If so, there's something wrong elsewhere!!! (By the way, when I use w, x, y and z, I mean that w is the "angle" component of the quaternion and x, y and z form the "vector" component.) (And cw1, cx1, cy1 and cz1 is the conjugate). Thanks both of you, Anthony |
| By: Martin Baker - martinbaker RE: Quaternions and Aircraft Roll Demand Cont 2005-10-26 08:18 |
| Perhaps by analogy with the 2D case, we could use: error angle = actual angle - required angle Or we could use: error angle = required angle - actual angle These are the same except it reverses which direction of error is considered positive and which is considered negative. In the 3D case addition is replaced with multiplication and multiplication by -1 is replaced by conjugate. So the equivalent is 3D might be: error quaternion = actual quaternion * conj(required quaternion) or error quaternion = required quaternion * conj(actual quaternion) depending on which direction of error we consider 'positive'. One result would be the conjugate of the other (which could be proved by the equation Andy gives) Martin |
| By: Andy Goldstein - andygoldstein RE: Quaternions and Aircraft Roll Demand Cont 2005-10-26 12:15 |
| The answer to the above two possibilities is "probably neither". The reason has to do with frame of reference. Anthony, you haven't said so, but I'm going to make a guess that you want your "difference angle" (i.e., the correction quaternion) expressed in the frame of reference of the airplane. (After all, that's how control inputs are applied - ailerons apply a roll along the *plane's* roll axis.) With that in mind, let's look very carefully at the rotation quaternions we're dealing with: Q1 = the plane's actual attitude, expressed in the external frame of reference. Q2 = the plane's desired attitude, expressed in the external frame of reference. Q3 = the difference, expressed in the plane's frame of reference. Expressed formally, the problem is defined as Q2 = Q1 x Q3 The reason for this ordering is that (1) Q3 is expressed in the plane's frame of reference, and therefore must be applied first, and (2) referring to my preceding post, rotations are composed by ordering the quaternions right to left. This equation is solved for Q3 simply by pre-multiplying by Q1* (conjugate of Q1), yielding Q1* x Q2 = Q1* x (Q1 x Q3) = (Q1* x Q1) x Q3) = Q3 Anthony, this matches the second of your computations in your last post. If that's the one you tried and you're not getting the right answer, then something is still wrong. First step would be to check your frames of reference again. If it's still not working for you I'll have to throw some code together quick and see if the actual reuslts match my theory... - Andy |
| By: Prospero999 - prospero999 RE: Quaternions and Aircraft Roll Demand Control* 2005-10-28 07:12 |
| Thanks guys, I'm taking a few days to absorb this. Cheers, Anthony |
