Software > Software

calculating orientation and location using Mahony quaternions

(1/3) > >>

I've been reading as much as I can but the information refuses to enter my brain. Time for me to ask for help . . .

I have an IMU spitting out 9 DoF of data. Given that, I'm trying to figure out the x/y/z and rotation coordinates over time using an Axon Mote microcontroller.

It appears what I need to use is the Mahony algorithm. It takes in that data and spits out four quaternions. But I'm not entirely sure what to do with those quaternions that I get.

1)
Given a series of calculated quaternions over time (using MahonyAHRS.c), how do I calculate the final x/y/z coordinates? And how do I determine the angular orientation? C example code would be sufficient.

2)
Second, looking at the top of his code, I see:

volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame

How do I calculate this so as to get my own initial constants? Is it just 180 degrees if I see 1, and 0 degrees if I see 0? Which quaternion for which axis of rotation?

3)
I also see this:

#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain

Am I correct to assume Mahony uses PI to control the error of the quaternions? Any tips on determining these beyond the usual guesswork/experimentation with my IMU?

jwatte:
I'm not familiar with the Mahony algorithm, but I am familiar with quaternions.

Unit Quaternions are a representation of a three-dimensional orientation. A unit vector multiplied by a quaternion will be taken from the "basis" orientation to the orientation represented by the quaternion.

From your questions, it sounds like you're not actually clear what an orientation transform is, though. An orientation transform can be thought of as a typical X/Y/Z vector "tripod" (red/green/blue arrows in most 3d software.) The default "world" or "base" orientation typically has X to the right, Y up, and Z out of the screen (but there are others -- like X east, Y north, and Z up in a geocentric coordinate space, for example.)
The tripod representing a quaternion would then have its "X" "Y" and "Z" arrows pointing in the direction they would be transformed to, if you put in the unit X, Y or Z vectors from the base orientation. For unit quaternions, this wlll be a rigid, non-scaling, non-shearing, non-translating transform -- similar to a 3x3 rotation matrix where all the rows (and columns) are unit length and orthogonal.

Quaternions are represented as a vector (xyz) and a scalar (w) and the two unit quaternions that represent "identity" (no change) have the vector 0,0,0, and the scalar 1 or -1. (Yes, quaternions cover the parameter space of 3D rotations twice, just to make things extra exciting :-)
I would assume that the quaternion (1,0,0,0) in your example represents identity -- no change relative to world. However, because the code doesn't say whether it puts w first or last (both are accepted conventions,) it's hard to tell. It could also be the 180-degree rotation around the x axis, represented by (x,y,z)==(1,0,0) and w == 1.

Note that a "180 degree rotation" is not well defined -- there exists an infinite number of rotations that rotate 180 degrees, and each of them will make the transformed input end up in a different orientation! For example, consider rotating 180 degreers around the X axis, versus the Y axis, versus the Z axis. The end results are very different.

A unit quaternion should have length 1, so sqrt(x*x+y*y+z*z+w*w) should be 1. Using this, you can calculate the quaternion representing a rotation of R around unit-length axis A as:
w = cos(R / 2)
xyz = A * scale (where scale makes the whole quaternion length 1 -- I'm too lazy to look up the function, but it's on Wikipedia and Mathworld and all over the web, really, and even pretty easy to derive yourself :-)

Finally, when you say "what is my x, y, z," what do you mean? X, Y, Z are typically used alone for translation, and quaternions are typically not used with translation at all. And when you say your "angular orientation," what angles do you mean? A quaternion is already a representation of orientation. Do you mean "heading, pitch, roll"? If so, relative to what coordinate frame? X axis as forward? Note that heading/pitch/roll relative to a fixed world reference behaves very unintuitively when you're not mainly pointing "forward."

--- Quote from: jwatte on March 03, 2013, 08:09:56 PM ---Finally, when you say "what is my x, y, z," what do you mean? X, Y, Z are typically used alone for translation, and quaternions are typically not used with translation at all. And when you say your "angular orientation," what angles do you mean? A quaternion is already a representation of orientation. Do you mean "heading, pitch, roll"? If so, relative to what coordinate frame? X axis as forward? Note that heading/pitch/roll relative to a fixed world reference behaves very unintuitively when you're not mainly pointing "forward."

--- End quote ---
I'm still going through your response, but wanted to clarify my question.

Assume my robot starts at position 0,0,0 (x,y,z coordinates) and has orientation a,a,a (heading, pitch, roll). It moves around a bunch, gets blown by the wind, goes up and down, etc., for a few minutes. Let's say I calculated the quaternions every 0.01s. Given that list of quaternions, how do I calculate the current x/y/z coordinates, and a,a,a?

I actually did quite a lot of reading of quaternions, but I don't really understand how to convert it to 'normal' rotational degrees from the horizontal.

The Mahony algorithm, to me, is a magic black box that takes IMU data as an input and then outputs the calculated quaternions.

I then just need to figure out how to relate that to robot rotation + translation so I can control the thrusters appropriately. But that's where I'm stuck . . . I'm sure there is code out there I can just copy/paste, but can't seem to find it . . .

jwatte:

--- Quote ---Let's say I calculated the quaternions every 0.01s. Given that list of quaternions, how do I calculate the current x/y/z coordinates, and a,a,a?
--- End quote ---

Given that unit quaternions only represent orientations, you cannot calculate x/y/z from that. You need to also integrate the forces. The quaternions (orientation of drone in world space) plus forces (acceleration of drone in world space) allow you to translate the forces to world space and then integrate to arrive at world-space position. If you have a quaternion Q that represent orientation (transform from world to drone space) then multiplying the force vector by Q' (conjugate) takes that vector from drone space to world space.

Also, again, "heading pitch roll" in world space is almost entirely useless. Heading is useful for knowing which compass direction "forward" points, but the pitch and roll meanings change as heading changes. Perhaps you want Tate-Bryant orientation order? (Each of heading, pitch, and roll are expressed relative to the one before it.) Even so, it's only useful for display, not typically for actual math. Turning the quaternion into a 3x3 rotation matrix and plotting the basis vectors would be a better visualization in my experience.

The varaible here that I know nothing about is the Mahony quaternions. There are ways you can use multiple non-unit quaternions to represent movement; perhaps that's what they are doing?