Mapping Rotations of Relative Axes to Fixed Axes

[DarthBane]

So I have been trying to figure out some orientation data that I gather from a triaxial gyroscope, and figure out my orientation using only initial conditions and angular velocity from the gyroscope's current axes.

The data is all relative to the current orientation, so if I rotate the device about the x-axis 90 degrees and then flip it to some other orientation and rotate again about the "new" x-axis 90 degrees, the data will remain the same. The axes of the gyroscope are fixed to its orientation, and I want to map these rotations back to a fixed coordinate system, namely the Earth's. I am working with the classic "yaw", "pitch", and "roll" angles, psi, theta, and phi respectively for aircraft orientation. Trying out a hobby UAV with my own coding.

The ωn terminology refers rotational velocity about axes n relative to the gyroscope, and then the greeks are for my fixed coordinate system that I have established
The current equations that I have are:

∂$\phi$(t)/∂t = ωx(t) + tan(θ(t-1))*( ωy(t)*sin($\phi$(t-1))) + ωz(t)*cos($\phi$(t-1));
∂θ(t)/∂t = ωy(t)*cos($\phi$(t-1)) - ωz(t)*sin($\phi$(t-1));
∂ψ(t)/∂t = sec(θ(t-1))*(ωy(t)*sin($\phi$(t-1)) + ωz(t)*cos($\phi$(t-1)));

Some of my rotations aren't quite coming out as expected, and I know that it isn't noise from the gyroscope, so I assume I have implemented something wrong with my rotations.

Any help would be appreciated, thanks in advance.

 

[D H]

Yep. You've misinterpreted what angular velocity means. It is not the derivatives of the Euler angles. Those derivatives do not form a vector. Angular velocity does form a vector. (Strictly speaking, it's a pseudo vector.) The angular velocity reported by a rate gyro is the angular velocity vector of the body that houses the gyro with respect to an non-rotating frame (inertial) frame but expressed in case frame coordinates.

So how to integrate angular velocity? Well, you can't. Not if you want a physically meaningful result.

What you can do is integrate orientation, represented as either a matrix or a unit quaternion. There's a mathematical relationship between the time derivative of a transformation/rotation matrix and angular velocity. The same applies to the time derivative of a unit quaternion used to represent transformation/rotation. What that relation is, exactly, depends on your interpretation of transformation/rotation and on how you represent things.

With matrices, you'll get something along the lines of $\dot T = \operatorname{Sk}(\vec\omega) T$, where T is the matrix and Sk(x) is the 3x3 skew symmetric matrix generated by the vector x. There might be a negative sign in that relation, and the skew symmetric matrix might follow rather than preceed the transformation matrix on the right hand side. Which is which once again depends on how you represent things.

Given the above, you can now numerically integrate the transformation matrix. Kind of, that is. If you use any standard integration technique you'll find that your integrated matrix quickly deviates from being an orthogonal matrix. The reason is that the differential equation ignores geometry. There are a number of tricks of the trade to make the integrated matrix be orthogonal that kinda/sorta work. They are just that, however: Tricks of the trade.

If you instead use quaternions to represent orientation you'll get something similar for that quaternion's time derivative with an additional factor of 1/2. The same basic problems apply: The expression might be negated and the multiplication might be opposite that depicted. Once again the problem of representation rears it's ugly head. The same problem with non-orthogonality also applies, but here the problem is that integrating a quaternion using standard techniques results in something that isn't a unit quaternion. The standard trick here is to normalize the result after each step. It's still a kludge.

There has been a huge amount of work over the last 15 or so years to address these kluges. The best thing to do is to use a technique that obeys the geometry of the problem. In this case, that is the group SO(3). This is a kind of a Lie group. The work that has been done is in terms of Lie group integration techniques. The mathematics is a bit intense, but the results are well worth it.

 

[DarthBane]

Thanks for the reply D H.

I think I get what you are saying for the most part, so forgive me if I am a bit thick in my reply:

I have been using simple Rotation matrices from SO(3), in order to rotate accelerometer data using the angles that I presented above. I iteratively rotate the accelerometer data using a rotation matrix and record the last Rotation matrix. I then form a new matrix based upon the new gyroscope data, and I have been experimenting with the equations above in order to form the angles that I could map back to the global coordinate system, but if that is incorrect I suppose I will have to go back to using simply the gyroscope data. Every iteration, I update my rotation matrix in order to create a mapping, but as you have said that it starts to diverge after a while. I have looked at several papers on using quaternions, but would like to avoid it if I can. They seem to be quite an elegant form of doing rotations and translations, but I would have to do more research into how to determine the axes of rotation based upon gyroscope data as well as the MATLAB toolbox is about a grand, but I suppose I could write my own program for doing quaternion multiplication.

I understand that the angular velocity from the gyrosopes are relative to the housing, but I am trying to figure out some way to map everything back to my starting coordinate system. Can I do this simply using the SO(3) rotation matrices, or would using quaternions be much simpler?

I don't mind digging into the math and would greatly appreciate any suggestions of papers or textbooks on matters of performing these kind of orientation mappings or the mathematics behind it.

I have been scouring the internet for a while on performing these kind of rotations, and it has been fairly unsuccessful, but I figured somewhere around here might be able to point me in the right direction.

Thanks

 

[D H]

Caveat: You haven't said what your application domain is or what accuracy requirements you have, so some of this might be over the top.

First off, there is no escaping that your problem domain is SO(3). There are a number of ways of representing orientation in three dimensional space, but they all are just that -- representations of SO(3). Every approach has benefits and drawbacks. There is no perfect scheme.

I, along with a number of others, like the unit quaternion representation, at least for integration. One drawback is that unit quaternions are not isomorphic to SO(3). They are a double map. A quaternion q and its additive inverse -q represent the exact same orientation. This is not a big problem. It usually is not a problem at all. What I do is to use quaternions to propagate state from one time step to the next and then compute alternate representations as required (but only once per time step). There's a whole lot to be said for using transformation matrices, so I always compute a transformation matrix from the quaternion. There's a whole lot less to be said for using Euler angles, so I only compute those on demand.

As far as references go, the book Quaternions and Rotation Sequences: A Primer With Applications to Orbits, Aerospace, and Virtual Reality by Jack B. Kuipers is hard to beat as an intro to the subject of using quaternions to represent transformations / rotations in three dimensional space. It goes into subjects essential to using quaternions as a representation of SO(3), including how the derivative relates to angular velocity.

With regard to propagation, the first step is to use that relation between angular velocity and the time derivative of your representation scheme of choice. The easiest thing to do is to ignore geometry. For example, if you are using RK4, just pretend that the canonical RK4 algorithm works just fine. At some point (usually quickly) you'll find that things diverge too much. The next step is to employ some kludge that "fixes" that divergence. For example, normalize the integrated quaternion after each time step and tada! you have a unit quaternion, by definition. This kludge isn't terrible. Multiple organizations have used this kludge for decades. This might well be as far as you need to go. This kludge isn't quite a kludge; it is valid in the limit of infinitesimal time steps.

The reason I call it a kludge is because of work over the last 15 years or so. There are much better ways to do things. For example, google "Runge-Kutta Munthe-Kaas". That's just one of several Lie group integration techniques (google that term, also) that have been developed in the last decade or two. Paying attention to geometry is almost always going to yield better results than will ignoring geometry or a kludgey kludge that kinda/sorta fixes problems in an ad hoc way.

One last point: You are dealing with measurements rather than truth data. Your gyro inevitable has errors. Multiple kinds of errors. A 0.01 radians/second rate might be reported as 0.0099 radians/second because your gyro has a scale factor error, a null rotation might consistently yield a non-zero because the gyro is biased, and worst of all, the gyro has random noise. (This is not anywhere near a complete list of your problems.) Build a sophisticated enough filter and you can detect things such as scale factor error and bias. Random noise: Your orientation is going to take a random walk.

What you need is (a) an independent reading of orientation, and (b) a scheme such as a Kalman filter that accounts for errors and for competing measurements. Designing a Kalman filter or some other adaptive technique that computes vehicle orientation is beyond the scope of an internet forum. It's appropriate material for a class, a book, a bunch of journal articles.

 

[DarthBane]

D. H., Thanks for the suggestions. The application is to track a UAV for about 30 seconds without using control feedback and to accuracy of about .5 meters while expecting the vehicle to fly no more than about 200 meters, so essentially autonomous UAV tracking. I am expecting the device to have quite a few drastic changes in orientation though, so I have to be able to track attitude pretty accurately.

I am looking at the RK methods for manifolds as well as Lie groups and believe that this will definitely be of benefit to me.

I am actually fairly well into doing the research for creating an EKF, or another KF variant, for correcting for error in my measurements. I will also be using calibrated magnetometer data as a secondary source for my EKF. I currently have a bunch of journal articles on quaternion based KF variants and intend to begin reading them.

This project has been quite interesting and fairly challenging as I am not quite done with my undergraduate degree, and have actually specialized in VLSI design. So... this is rather foreign to me, hence why I have come to the PF for help.

Thanks, and any further thoughts now that you have seen my problem set?

 

[D H]

What's your sampling rate on the rate gyro? If it's fast enough, you may be get away without resorting to Lie group techniques. If it's extremely fast, your flight software might not even be able to keep up if you use a Lie group technique, depending on what your underlying integration technique is.

BTW, for RK4 I looked at three alternative Lie group approaches: RKMK (Runge-Kutta Munthe-Kaas), Crouch Grossman, and commutator-free. The commutator-free approach was the fastest and most accurate with my test cases. (YMMV.) As far as accuracy is concerned, all three approaches beat the pants off standard RK4 where one integrates the quaternion derivative and then renormalizes the result to a unit quaternion. The key advantage of that simple RK4 approach is that it is less intensive computationally. There is no exponential map, no dexpinv. The key advantage of the improved accuracy from the Lie group techniques is that one can take significantly larger time steps and stay within some desired error bounds. Bigger time steps in turn reduces CPU load and reduces random walk errors. OTOH, you have to operate at some minimal speed lest you risk losing observability and/or controllability.

There is one problem with using quaternions in a Kalman filter. It's not near as big a problem as is using 3x3 matrices, but it's still there. The problem is that rotation in three space has three degrees of freedom. That matrix, with nine parameters, is huge overkill. The 9x9 covariance matrix is necessarily singular because you only have three degrees of freedom. You probably won't get a matrix that truly is singular. You'll instead get one that is incredibly ill conditioned. This means the standard Kalman filter algorithms are going to explode. That potential for things going awry because the covariance matrix is ill-conditioned still exists with quaternions because quaternions have four elements.

You have papers on using quaternions for an attitude filter, so I presume you're aware of this problem. It's fairly easy to overcome with only one extra parameter than degrees of freedom. Some people are moving to modified Rodrigues parameters to get around this problem.

 

[DarthBane]

Sampling rate is only 200 Hz. I have just been integrating simply by addition of previous gyro reading multiplied by sampling time: angle = angle + gyro*.005. This is obviously a very simple and most likely very wrong way of doing things. So I shall look into advanced integration techniques. I haven't seen an RK methods in two years, so I had completely forgot about the idea until you mentioned it. This also gives away that I haven't really done much of the research into the actual differential equations I guess... So need to look more into those then and create an RK scheme for it.

I recently acquired the papers and am just beginning to read them so I have not seen that issue yet. I apologize if I sound quite thick headed. Just haven't been in the swing of things for long with this. I will look into the Rodriguez parameters.

Thanks again.

Anything else that comes to mind when speaking with a novice to the material, I know I still have good sized gaps in my understanding of the material?

 

[DrZoidberg]

I didn't understand much of this discussion but I played around with 3D graphics once creating a very simple 3D game/simulation and I also used matrices for that. Each object in the game had two matrices, one for representing the current orientation and one for the angular velocity. So in each iteration I just had to multiply the orientation matrix by the ang.vel. matrix to get the new orientatation. The virtual camera of course also had an orientation matrix. With the mouse I could then rotate the camera and that rotation was relative to the camera. So to do that I just created a matrix representing the rotation rel. to the camera and then multiplied that matrix with the orientation of the camera. Originally I had the multiplication reversed (orientation times rotation matrix) and got a rotation relative to the absolute coordinate system which is not what I wanted.

 

[D H]

Anything else that comes to mind when speaking with a novice to the material, I know I still have good sized gaps in my understanding of the material?

1. Keep it simple.
2. Watch out for representation issues.
3. Rodrigues rotation formula.

Keep it simple: I'm guilty of giving too much information on subjects such as this. It's something I have decades of experience on. I forget that I too was a newbie once. If this is an undergrad research you'll want to keep it as simple as you can. Fortunately, 30 seconds isn't all that long a span of time. (And hopefully the instructor who's grading this isn't going to send the UAV into a snap roll during that short span of time.) If it's a masters level project, you'll of course have to dive in quite a bit deeper. But you still need to keep it simple.

Watch out for representation issues: You're going to see apparently contradictory mathematics in the papers and books you find on this. Oftentimes, there is no contradiction. It's just different representations. Are you modeling orientation as a transformation or rotation (others call it passive transformation versus active transformation)? Vectors as row vectors or column vectors? Quaternions as left or right quaternions? (Note: This has nothing to do with handedness of a coordinate system. It's the difference between qvq* and q*vq. The unconjugated quaternion can be placed on the left or on the right. Your choice.)

Rodrigues rotation formula: This makes for an easy computation of the exponential map, something you'll need if you want to use a Lie group technique. The wikipedia article on this subject is okay. Not good, not bad. Just OK.

Each object in the game had two matrices, one for representing the current orientation and one for the angular velocity. So in each iteration I just had to multiply the orientation matrix by the ang.vel. matrix to get the new orientatation.

Angular velocity matrix? That doesn't make sense as is. Angular velocity can be represented as a vector (better: pseudo vector) or as a 3x3 skew symmetric matrix. In either case, it's not unitless and it's not an orthogonal matrix.

I suspect you meant the exponential map of ωΔt, which is a unitless orthogonal matrix. That exponential map is easy to compute for rotations in three space via the Rodrigues rotation formula. If that's what you used, what you did is the simplest of the SO(3) Lie group integration techniques. This is analogous to using Euler's method to integrate translational velocity to yield position. It has the benefits of Euler's method (easy to implement) and the drawbacks of Euler's method (lousy performance).

 

[DarthBane]

This is at a Master's level work, and I count on every trick in the book being thrown at me, including crashing the blooming thing into a wall for an impact, spins, rolls, and dives. A snap roll wouldn't surprise me at all or something similar. I switched majors from ME to a EE so I am taking a 5th year to finish my undergrad, but already doing some masters level work with Aerospace material.

I am actually enjoying the aerospace mathematics much more so than I have my EE curriculum. As much as I enjoy analog circuit design, I have a lot more fun with the advanced mathematics for this modeling.

Thanks for the tip on representation issues. I usually look stuff up, but sometimes will get a bit confused. So I'll keep that in mind.

I have done a little look-up on the Rodrigues Rotations, and will look further into it.

Thanks for your help. I'll post if I have more trouble or if I make some good progress. Thanks!